diff --git a/.gitignore b/.gitignore index 0d20b648..ebcc5fe7 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,4 @@ *.pyc +build +log +install \ No newline at end of file diff --git a/Dockerfile-foxy b/Dockerfile-foxy new file mode 100644 index 00000000..4fbf753d --- /dev/null +++ b/Dockerfile-foxy @@ -0,0 +1,35 @@ +FROM ros:foxy-ros-core + +# python3-osrf-pycommon has to be installed manually for python3-catkin-tools to work (see https://github.com/catkin/catkin_tools/issues/594) +RUN apt-get update \ + && apt-get install -y --no-install-recommends build-essential python3-rosdep python3-catkin-lint python3-catkin-tools python3-osrf-pycommon \ + && rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/* + +# Install pre-commit hooks to /root/.cache/pre-commit/ +RUN apt-get update -qq \ + && apt-get install -y -qq --no-install-recommends git python3-pip ruby shellcheck clang-format-10 python3-catkin-lint \ + && rm -rf /var/lib/apt/lists/* +RUN pip3 install pre-commit +RUN mkdir -p /tmp/pre-commit +COPY .pre-commit-config.yaml /tmp/pre-commit/ +RUN cd /tmp/pre-commit \ + && git init \ + && pre-commit install-hooks \ + && rm -rf /tmp/pre-commit + +# Create ROS workspace +COPY . /ws/src/mir_robot +WORKDIR /ws + +# Use rosdep to install all dependencies (including ROS itself) +RUN rosdep init \ + && rosdep update \ + && apt-get update \ + && DEBIAN_FRONTEND=noninteractive rosdep install --from-paths src -i -y --rosdistro foxy \ + && rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/* + +RUN /bin/bash -c "source /opt/ros/foxy/setup.bash && \ + colcon build && \ + catkin config --install -j 1 -p 1 && \ + catkin build --limit-status-rate 0.1 --no-notify && \ + catkin build --limit-status-rate 0.1 --no-notify --make-args tests" diff --git a/README.md b/README.md index a469e75c..74077d52 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,15 @@ +# Disclaimer + +**This package is intended to be a migration package of [mir_robot using ros1](https://github.com/dfki-ric/mir_robot) to [ros galactic](https://docs.ros.org/en/galactic/Installation.html). +It is currently still under heavy development - to be used with caution!** +The following packages have been migrated to galactic: + +- [x] mir_description +- [x] mir_driver +- [x] mir_gazebo +- [x] mir_msgs +- [x] mir_navigation + mir_driver ========== @@ -13,7 +25,7 @@ missing feature in this software, please report it on the Supported MiR robots and software versions ------------------------------------------ -This repo has been confirmed to work with the following robots: + Package overview @@ -40,24 +52,21 @@ Package overview Installation ------------ - + ### Preliminaries -If you haven't already installed ROS on your PC, you need to add the ROS apt +#### ROS2 +If you haven't already [installed ROS2](https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html) on your PC, you need to add the ROS2 apt repository. This step is necessary for either binary or source install. -``` -sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' -wget http://packages.ros.org/ros.key -O - | sudo apt-key add - -sudo apt-get update -qq -``` + ### Source install @@ -76,80 +87,124 @@ For a source install, run the commands below instead of the command from the "binary install" section. ```bash -# create a catkin workspace -mkdir -p ~/catkin_ws/src -cd ~/catkin_ws/src/ +# create a ros2 workspace +mkdir -p ~/ros2_ws/src +cd ~/ros2_ws/ + +# clone mir_robot into the ros2 workspace +git clone -b galactic-devel https://github.com/relffok/mir_robot src/mir_robot -# clone mir_robot into the catkin workspace -git clone -b noetic https://github.com/dfki-ric/mir_robot.git +# use vcs to fetch linked repos +vcs import < src/mir_robot/ros2.repos src --recursive # use rosdep to install all dependencies (including ROS itself) -sudo apt-get update -qq -sudo apt-get install -qq -y python-rosdep -sudo rosdep init +sudo apt update +sudo apt install -y python3-rosdep rosdep update -rosdep install --from-paths ./ -i -y --rosdistro noetic +rosdep install --from-paths src --ignore-src -r -y --rosdistro galactic -# build all packages in the catkin workspace -source /opt/ros/noetic/setup.bash -catkin_init_workspace -cd ~/catkin_ws -catkin_make -DCMAKE_BUILD_TYPE=RelWithDebugInfo +# build all packages in the workspace +source /opt/ros/galactic/setup.bash +cd ~/ros2_ws +colcon build ``` -In case you encounter problems, please compare the commands above to the build -step in [`.travis.yml`](.travis.yml); that should always have the most -recent list of commands. +You must source the workspace in each terminal you want to work in: -You should add the following line to the end of your `~/.bashrc`, and then +```bash +source ~/ros2_ws/install/setup.bash +``` +Alternatively you can add the following line to the end of your `~/.bashrc`, and then close and reopen all terminals: ```bash -source ~/catkin_ws/devel/setup.bash +echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc ``` Gazebo demo (existing map) -------------------------- +### Option 1: Launching the modules separately + ```bash -### gazebo: -roslaunch mir_gazebo mir_maze_world.launch -rosservice call /gazebo/unpause_physics # or click the "start" button in the Gazebo GUI +### gazebo +ros2 launch mir_gazebo mir_gazebo_launch.py world:=maze + +### localization (existing map) +ros2 launch mir_navigation amcl.py use_sim_time:=true map:=$(ros2 pkg prefix mir_navigation)/share/mir_navigation/maps/maze.yaml +### navigation +ros2 launch mir_navigation navigation.py use_sim_time:=true +``` + +### Option 2: Use combined launch file + +```bash +### combined launch file +ros2 launch mir_navigation mir_nav_sim_launch.py world:=maze + +### (opt) Show possible launch arguments: +ros2 launch mir_gazebo mir_gazebo_launch.py --show-args +``` + + -Now, you can use the "2D Nav Goal" tool in RViz to set a navigation goal for move_base. + +* **HEADLESS MODE:** To run the simulation without the gazebo graphical interface (performance improvement) add: + + gui:=false + + +Initialize the position with "2D Pose Estimate". You should see sensor inputs and costmap by now. +Now, you can use the "2D Goal Pose" tool in RViz to set a navigation goal for move_base + Gazebo demo (mapping) --------------------- +### Option 1: Launching the modules separately + ```bash ### gazebo: -roslaunch mir_gazebo mir_maze_world.launch -rosservice call /gazebo/unpause_physics # or click the "start" button in the Gazebo GUI +ros2 launch mir_gazebo mir_gazebo_launch.py world:=maze -### mapping: -roslaunch mir_navigation hector_mapping.launch +### mapping (slam_toolbox) +ros2 launch mir_navigation mapping.py use_sim_time:=true slam_params_file:=$(ros2 pkg prefix mir_navigation)/share/mir_navigation/config/mir_mapping_async_sim.yaml -# navigation: -roslaunch mir_navigation move_base.xml with_virtual_walls:=false -rviz -d $(rospack find mir_navigation)/rviz/navigation.rviz +### navigation (optional) +ros2 launch mir_navigation navigation.py use_sim_time:=true ``` +### Option 2: Use combined launch file + +Instead of launching the 3 modules seperately, you can also use a combined launch file: + +```bash +### combined launch file: +ros2 launch mir_navigation mir_mapping_sim_launch.py +``` + +* **NAVIGATION**: Navigation is disabled per default. If you like to teleop the robot using nav2 add: + + navigation_enabled:=true + + + Running the driver on the real robot @@ -215,23 +270,29 @@ Running the driver on the real robot * open mir.com (192.168.12.20) in Chrome (!) * log in (admin/mir4you) - ### Synchronize system time The internal robot PC's is not synchronized (for example via NTP), so it tends to get out of sync quickly (about 1 second per day!). This causes TF transforms timing out etc. and can be seen using `tf_monitor` (the "Max Delay" is about -3.3 seconds, but should be less than 0.1 seconds): +3.3 seconds in the following example, but should be less than 0.1 seconds): +#### **Determine delay using ROS1** (optional) + +Since MiR is running a roscore (ROS1), the following tf_monitor can be excecuted after sourcing ROS1 (i.e. noetic) first: + +```bash +source /opt/ros/noetic/setup.bash +export ROS_MASTER_URI=http://192.168.12.20:11311 ``` + + +```bash $ rosrun tf tf_monitor Frames: Frame: /back_laser_link published by unknown_publisher Average Delay: 3.22686 Max Delay: 3.34766 Frame: /base_footprint published by unknown_publisher Average Delay: 3.34273 Max Delay: 3.38062 Frame: /base_link published by unknown_publisher Average Delay: 3.22751 Max Delay: 3.34844 -Frame: /front_laser_link published by unknown_publisher Average Delay: 3.22661 Max Delay: 3.34159 -Frame: /imu_link published by unknown_publisher Average Delay: 3.22739 Max Delay: 3.34738 -Frame: /odom published by unknown_publisher Average Delay: 3.16493 Max Delay: 3.28667 [...] All Broadcasters: @@ -239,11 +300,33 @@ Node: unknown_publisher 418.344 Hz, Average Delay: 0.827575 Max Delay: 3.35237 Node: unknown_publisher(static) 465.362 Hz, Average Delay: 0 Max Delay: 0 ``` -To fix this: +#### **Determine delay using ROS2** + +If you don't have a ROS1 distro installed, you'll need to run the `mir_driver` first and execute the following once a connection is established: + +```bash +ros2 run tf2_ros tf2_monitor +``` + + +#### **Fix time synchronization manually:** + +* In the Mir dashboard (mir.com in the Mir-Wifi), go to "Service" -> "Configuration" -> "System settings" -> "Time settings" -> "Set device time on robot" +Use **load from device** to sync with the system time! + +#### **Fix time synchronization using ROS2:** + +From the package `mir_restapi` a node called `mir_restapi_server` can be run, which can execute a time sync REST API call from the driver's host machine to the Mir's host. +* Launch the node with the API key and mir hostname's IP address + + ros2 run mir_restapi mir_restapi_server --ros-args -p mir_hostname:='MIR_IP_ADDR' -p mir_restapi_auth:='YOUR_API_KEY' +* Call the time sync service from terminal by invoking + + ros2 service call /mir_100_sync_time std_srvs/Trigger -* go to "Service" -> "Configuration" -> "System settings" -> "Time settings" -> "Set device time on robot" +#### **After time sync** -Afterwards, the ROS software on the robot will restart, so you'll have to start `move_base` again (see below). +Keep in mind, that the time sync causes the mir_bridge to freeze. Therefore online time syncs are not recommended. ### Start `move_base` on the robot @@ -263,13 +346,135 @@ If the robot's localization is lost: * go to "Service" -> "Command view" -> "Set start position" and click + drag to current position of robot in the map * click "Adjust" - ### Start the ROS driver ```bash -roslaunch mir_driver mir.launch +ros2 launch mir_driver mir_launch.py +``` + +* The driver automatically launches **rviz** to visualize the topics and sensor messages. To disable use `rviz_enabled:=false` as a launch argument +* The driver automatically launches a seperate **teleop** window to manually move the robot using your keyboard. To disable use `teleop_enabled:=false` as a launch argument + +### Mapping on MiR + +### Option 1: Launching the modules separately + +```bash +### driver: +ros2 launch mir_driver mir_launch.py + +### mapping (slam_toolbox) +ros2 launch mir_navigation mapping.py use_sim_time:=false slam_params_file:=$(ros2 pkg prefix mir_navigation)/share/mir_navigation/config/mir_mapping_async.yaml + +### navigation (optional) +ros2 launch mir_navigation navigation.py use_sim_time:=false +``` + +### Option 2: Use combined launch file +```bash +### combined launch file: +ros2 launch mir_navigation mir_mapping_launch.py +``` + +### Navigation on MiR + +### Option 1: Launching the modules separately + +```bash +### driver: +ros2 launch mir_driver mir_launch.py + +### localization (amcl) +ros2 launch mir_navigation amcl.py use_sim_time:=false map:={path to existing map} + +### navigation +ros2 launch mir_navigation navigation.py use_sim_time:=false +``` + +### Option 2: Use combined launch file +```bash +### combined launch file: +ros2 launch mir_navigation mir_nav_launch.py map:={path to /name of existing map} ``` +On Mapping +------------------------------------ +As mentioned before, you can launch the differnet modules seperately or use the combined launch commands below: + +In **Simulation** run: + + ros2 launch mir_navigation mir_mapping_sim_launch.py + +On **MiR** run: + + ros2 launch mir_navigation mir_mapping_launch.py + +Both commands launch the simulation / driver and SLAM node. + +## How to map +To save the created map, use the rviz plugin **"Save Map"** and **"Serialize Map"**. From time to time segmentation faults or timeouts occur, so make sure your map is saved before shutting down the connection. + +![](doc/img/save_map.png) + +### Refine existing map +The given launch commands will create a fresh new map of the environment. If you like to adapt an existing map (must be serialized!) you can **deserialize it** using the rviz slam_toolbox plugin. + +Select map to deserialize | and continue mapping +:--------------:|:--------------: +![](doc/img/deseralize_map1.png) | ![](doc/img/deseralize_map2.png) + + +### Helpful launch arguments + +* **NAVIGATION**: Navigation is disabled per default. If you like to teleoperate the robot using nav2 add: + + navigation_enabled:=true + +Mapping.. | ..using nav2 +:--------------:|:--------------: +![](doc/img/mapping1.png) | ![](doc/img/mapping2.png) + + +On Localization and Navigation +------------------------------------ +As mentioned before, you can launch the differnet modules seperately or use the combined launch commands below: + +In **Simulation** run: + + ros2 launch mir_navigation mir_nav_sim_launch.py + +On **MiR** run: + + ros2 launch mir_navigation mir_nav_launch.py map:={path to existing map} + +Both commands launch the simulation / driver and localization (amcl) using an existing map. + +### Helpful launch arguments +* **MAP**: In Simulation the map defaults to the maze map. On the real robot a map should be passed via the ``map`` argument + + map:= {path_to_map_yaml} + world:={world_file} # simulation only: add respective world + +### Workflow + +Once the simulation / driver is running and rviz is started, you need to **set the initial pose** on the map. This doesnt have to be accurate, just a reference and amcl will do the refinement. To refine, **move the robot** around a little using the teleop window and the scan will eventually match the map. + +Initialize Pose | Drifted pose | Refined pose +:--------------:|:--------------:|:--------------: +![](doc/img/initial_pose1.png) |![](doc/img/initial_pose2.png)|![](doc/img/initial_pose3.png) + + +Using a namespace +------------------------------------ + +When using a namespace for your robot, add the ``--ros-args -p namespace:=my_namespace`` to the launch files. +The driver, description and gazebo packages work out of the box. +However, to use the navigation stack the corresponding config.yaml files need to be adapted to match the renamed topic. + +The navigation ``cmd_vel`` topic is automatically remapped by adding the namespace in the ``navigation.py`` launch file. + + -Travis - Continuous Integration + diff --git a/mir_driver/src/mir_driver/__init__.py b/doc/COLCON_IGNORE similarity index 100% rename from mir_driver/src/mir_driver/__init__.py rename to doc/COLCON_IGNORE diff --git a/doc/img/deseralize_map1.png b/doc/img/deseralize_map1.png new file mode 100644 index 00000000..9ab495a7 Binary files /dev/null and b/doc/img/deseralize_map1.png differ diff --git a/doc/img/deseralize_map2.png b/doc/img/deseralize_map2.png new file mode 100644 index 00000000..103f9ec7 Binary files /dev/null and b/doc/img/deseralize_map2.png differ diff --git a/doc/img/initial_pose1.png b/doc/img/initial_pose1.png new file mode 100644 index 00000000..d82eae66 Binary files /dev/null and b/doc/img/initial_pose1.png differ diff --git a/doc/img/initial_pose2.png b/doc/img/initial_pose2.png new file mode 100644 index 00000000..30b2c70e Binary files /dev/null and b/doc/img/initial_pose2.png differ diff --git a/doc/img/initial_pose3.png b/doc/img/initial_pose3.png new file mode 100644 index 00000000..5cd2db25 Binary files /dev/null and b/doc/img/initial_pose3.png differ diff --git a/doc/img/mapping1.png b/doc/img/mapping1.png new file mode 100644 index 00000000..a32b266a Binary files /dev/null and b/doc/img/mapping1.png differ diff --git a/doc/img/mapping2.png b/doc/img/mapping2.png new file mode 100644 index 00000000..aca1fe97 Binary files /dev/null and b/doc/img/mapping2.png differ diff --git a/doc/img/nav_local1.png b/doc/img/nav_local1.png new file mode 100644 index 00000000..794a924e Binary files /dev/null and b/doc/img/nav_local1.png differ diff --git a/doc/img/nav_local2.png b/doc/img/nav_local2.png new file mode 100644 index 00000000..97a97916 Binary files /dev/null and b/doc/img/nav_local2.png differ diff --git a/doc/img/save_map.png b/doc/img/save_map.png new file mode 100644 index 00000000..12c382e1 Binary files /dev/null and b/doc/img/save_map.png differ diff --git a/mir_actions/CHANGELOG.rst b/mir_actions/CHANGELOG.rst deleted file mode 100644 index 75e5ea22..00000000 --- a/mir_actions/CHANGELOG.rst +++ /dev/null @@ -1,52 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mir_actions -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.3 (2021-06-11) ------------------- -* Merge branch 'melodic-2.8' into noetic -* Remove RelativeMove action - It was removed in MiR software 2.4.0. -* Update mir_actions to MiR 2.8.3 -* Adjust to changed MirMoveBase action (MiR >= 2.4.0) - See `#45 `_. -* Contributors: Martin Günther - -1.1.2 (2021-05-12) ------------------- - -1.1.1 (2021-02-11) ------------------- -* Contributors: Martin Günther - -1.1.0 (2020-06-30) ------------------- -* Initial release into noetic -* Contributors: Martin Günther - -1.0.6 (2020-06-30) ------------------- -* Set cmake_policy CMP0048 to fix warning -* Contributors: Martin Günther - -1.0.5 (2020-05-01) ------------------- - -1.0.4 (2019-05-06) ------------------- -* Update mir_msgs and mir_actions to MiR 2.3.1 -* Contributors: Martin Günther - -1.0.3 (2019-03-04) ------------------- - -1.0.2 (2018-07-30) ------------------- - -1.0.1 (2018-07-17) ------------------- - -1.0.0 (2018-07-12) ------------------- -* Initial release -* Contributors: Martin Günther diff --git a/mir_actions/CMakeLists.txt b/mir_actions/CMakeLists.txt deleted file mode 100644 index 007f54c8..00000000 --- a/mir_actions/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 3.5.1) -cmake_policy(SET CMP0048 NEW) -project(mir_actions) - -find_package(catkin REQUIRED COMPONENTS - actionlib - geometry_msgs - message_generation - nav_msgs -) - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -# Generate actions in the 'action' folder -add_action_files( - FILES - MirMoveBase.action -) - -# Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - geometry_msgs - nav_msgs -) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - CATKIN_DEPENDS - actionlib - geometry_msgs - message_runtime - nav_msgs -) diff --git a/mir_actions/action/MirMoveBase.action b/mir_actions/action/MirMoveBase.action deleted file mode 100644 index 811e1359..00000000 --- a/mir_actions/action/MirMoveBase.action +++ /dev/null @@ -1,118 +0,0 @@ -#goal definition -#move type -int16 BASE_MOVE = 0 -int16 GLOBAL_MOVE = 1 -int16 RELATIVE_MOVE = 2 -int16 RELATIVE_MARKER_MOVE = 3 -int16 DOCKING_MOVE = 4 -int16 DOCKING_GLOBAL_MOVE = 5 -int16 move_task - -#shared parameters -geometry_msgs/PoseStamped target_pose - -#global move parameters -float64 goal_dist_threshold -float64 goal_orientation_threshold -nav_msgs/Path path -float32 max_plan_time -bool clear_costmaps -bool pause_command -bool continue_command - -#relative move parameters -float64 yaw -bool collision_detection -bool collision_avoidance -float64 disable_collision_check_dist -float64 max_linear_speed -float64 max_rotational_speed -float64 pid_dist_offset -float64 target_offset -bool only_collision_detection -float64 timeout - -#docking move parameters -int32 pattern_type -int32 pattern_value -bool only_track -bool same_goal -string pose_frame -geometry_msgs/Pose2D pose -geometry_msgs/Pose2D offset -float64 bar_length -float64 bar_distance -float64 shelf_leg_asymmetry_x -float64 tolerance - ---- -#result definition - -#shared states -int16 UNDEFINED = 0 -int16 GOAL_REACHED = 1 -int16 FAILED = -1 - -#global move states -int16 MARKER_VISIBLE = 2 -int16 FAILED_NO_PATH = -2 -int16 FAILED_GOAL_IN_STATIC_OBSTACLE = -3 -int16 FAILED_GOAL_IN_FORBIDDEN_AREA = -4 -int16 FAILED_GOAL_IN_DYNAMIC_OBSTACLE = -5 -int16 FAILED_ROBOT_IN_COLLISION = -6 -int16 FAILED_ROBOT_IN_FORBIDDEN_AREA = -7 -int16 FAILED_UNKNOWN_TRAILER = -8 -int16 FAILED_TO_PASS_GLOBAL_PLAN = -9 -int16 FAILED_NO_VALID_RECOVERY_CONTROL = -10 -int16 FAILED_UNKNOWN_PLANNER_ERROR = -11 -int16 FAILED_ROBOT_OSCILLATING = -12 -int16 FAILED_SOFTWARE_ERROR = -13 - -#relative move states -int16 FAILED_TIMEOUT = -14 -int16 FAILED_COLLISION = -15 -int16 INVALID_GOAL = -16 - -#docking move states -int16 FAILED_MARKER_TRACKING_ERROR = -17 - -#shared results -int16 end_state -geometry_msgs/PoseStamped end_pose - -#docking results -geometry_msgs/Pose2D pose - -#feedback for UI -string message - ---- -#feedback -#shared -int8 NOT_READY = -1 - -#global move states -int8 PLANNING = 0 -int8 CONTROLLING = 1 -int8 CLEARING = 2 - -#relative move states -int8 DOCKING = 3 -int8 COLLISION = 4 - -#shared feedback -int8 state - -#global move feedback -geometry_msgs/PoseStamped base_position - -#relative move feedback -geometry_msgs/PoseStamped current_goal -geometry_msgs/PoseStamped dist_to_goal - -#docking move feedback -#int8 DOCKING = 0 -#int8 COLLISION = 1 -#int8 state -geometry_msgs/Pose2D pose -bool marker_inversion diff --git a/mir_actions/package.xml b/mir_actions/package.xml deleted file mode 100644 index 205f7f97..00000000 --- a/mir_actions/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - mir_actions - 1.1.3 - Action definitions for the MiR100 robot - - Martin Günther - Martin Günther - - BSD - - https://github.com/dfki-ric/mir_robot - https://github.com/dfki-ric/mir_robot - https://github.com/dfki-ric/mir_robot/issues - - catkin - - actionlib - geometry_msgs - nav_msgs - - message_generation - message_runtime - diff --git a/mir_description/CMakeLists.txt b/mir_description/CMakeLists.txt index a1c28de8..de270ca3 100644 --- a/mir_description/CMakeLists.txt +++ b/mir_description/CMakeLists.txt @@ -1,32 +1,38 @@ -cmake_minimum_required(VERSION 3.5.1) -cmake_policy(SET CMP0048 NEW) +cmake_minimum_required(VERSION 3.5) project(mir_description) -find_package(catkin REQUIRED COMPONENTS - roslaunch -) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -################################### -## catkin specific configuration ## -################################### -catkin_package() - -############# -## Install ## -############# - -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - config - launch - meshes - rviz - urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(xacro REQUIRED) +find_package(robot_state_publisher REQUIRED) +find_package(gazebo_ros REQUIRED) +find_package(rviz2 REQUIRED) +find_package(urdf REQUIRED) +find_package(xacro REQUIRED) + +install( + DIRECTORY urdf rviz meshes config launch + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() -roslaunch_add_file_check(launch) diff --git a/mir_description/config/diffdrive_controller.yaml b/mir_description/config/diffdrive_controller.yaml index 49e5a2ce..f4b0dae3 100644 --- a/mir_description/config/diffdrive_controller.yaml +++ b/mir_description/config/diffdrive_controller.yaml @@ -7,7 +7,7 @@ mobile_base_controller: # These covariances are exactly what the real MiR platform publishes pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0] twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0] - enable_odom_tf: false + enable_odom_tf: true # Wheel separation and diameter. These are both optional. # diff_drive_controller will attempt to read either one or both from the @@ -24,7 +24,7 @@ mobile_base_controller: # frame_ids (same as real MiR platform) base_frame_id: $(arg prefix)base_footprint # default: base_link - odom_frame_id: $(arg prefix)odom # default: odom + odom_frame_id: $(arg prefix)odom # default: odom # Velocity and acceleration limits # Whenever a min_* is unspecified, default to -max_* diff --git a/mir_description/launch/mir_debug_urdf.launch b/mir_description/launch/mir_debug_urdf.launch deleted file mode 100644 index 18814186..00000000 --- a/mir_description/launch/mir_debug_urdf.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/mir_description/launch/mir_display_launch.py b/mir_description/launch/mir_display_launch.py new file mode 100644 index 00000000..03c8ac3e --- /dev/null +++ b/mir_description/launch/mir_display_launch.py @@ -0,0 +1,40 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + + mir_description_dir = get_package_share_directory('mir_description') + rviz_config_file = os.path.join( + mir_description_dir, 'rviz', 'mir_description.rviz') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'joint_state_publisher_enabled', + default_value='true', + description='Enable to publish joint states using joint state publisher'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_description_dir, 'launch', 'mir_launch.py')), + launch_arguments={ + 'joint_state_publisher_enabled': + LaunchConfiguration('joint_state_publisher_enabled'), + }.items() + ), + + Node( + package='rviz2', + executable='rviz2', + arguments=['-d', rviz_config_file], + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}] + ) + + ]) diff --git a/mir_description/launch/mir_launch.py b/mir_description/launch/mir_launch.py new file mode 100644 index 00000000..0d99a947 --- /dev/null +++ b/mir_description/launch/mir_launch.py @@ -0,0 +1,62 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetLaunchConfiguration +from launch_ros.actions import Node +import xacro + + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + mir_description_dir = get_package_share_directory('mir_description') + + def create_robot_description(context): + ns = context.launch_configurations['namespace'] + if ns.startswith('/'): + ns = ns[1:] + urdf_dir = os.path.join(mir_description_dir, 'urdf') + xacro_file = os.path.join(urdf_dir, 'mir.urdf.xacro') + doc = xacro.process_file(xacro_file, mappings={'tf_prefix': ns}) + robot_desc = doc.toprettyxml(indent=' ') + return [SetLaunchConfiguration('robot_description', robot_desc)] + + return LaunchDescription([ + + DeclareLaunchArgument( + 'prefix', + default_value='', + description='Robot prefix'), + + DeclareLaunchArgument( + 'namespace', + default_value='', + description='Namespace to push all topics to'), + + DeclareLaunchArgument( + 'joint_state_publisher_enabled', + default_value='false', + description='Enable to publish joint states using joint state publisher'), + + OpaqueFunction(function=create_robot_description), + + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[{'use_sim_time': use_sim_time, + 'robot_description': LaunchConfiguration('robot_description')}], + namespace=LaunchConfiguration('namespace'), + ), + + Node( + condition=IfCondition(LaunchConfiguration('joint_state_publisher_enabled')), + package='joint_state_publisher', + namespace=LaunchConfiguration('namespace'), + executable='joint_state_publisher') + + ]) diff --git a/mir_description/launch/upload_mir_urdf.launch b/mir_description/launch/upload_mir_urdf.launch deleted file mode 100644 index c6c94946..00000000 --- a/mir_description/launch/upload_mir_urdf.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/mir_description/package.xml b/mir_description/package.xml index 3de55b78..ca5abf91 100644 --- a/mir_description/package.xml +++ b/mir_description/package.xml @@ -1,5 +1,5 @@ - + mir_description 1.1.3 URDF description of the MiR100 robot @@ -13,17 +13,21 @@ https://github.com/dfki-ric/mir_robot https://github.com/dfki-ric/mir_robot/issues - catkin + ament_cmake - roslaunch - - diff_drive_controller - gazebo_ros_control - joint_state_controller joint_state_publisher - position_controllers robot_state_publisher - rviz + gazebo_ros + rviz2 urdf xacro + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/mir_description/rviz/mir_description.rviz b/mir_description/rviz/mir_description.rviz index c17b0416..d1078638 100644 --- a/mir_description/rviz/mir_description.rviz +++ b/mir_description/rviz/mir_description.rviz @@ -1,42 +1,37 @@ Panels: - - Class: rviz/Displays - Help Height: 81 + - Class: rviz_common/Displays + Help Height: 138 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 + - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 535 - - Class: rviz/Selection + Tree Height: 908 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -48,9 +43,31 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -139,8 +156,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + us_1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + us_2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true @@ -148,63 +172,79 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true Fixed Frame: base_footprint Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - 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 + 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: /clicked_point + 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/Orbit - Distance: 2.20704937 + Class: rviz_default_plugins/Orbit + Distance: 4.124688148498535 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.023150593 - Y: -0.0341755934 - Z: -2.6775524e-09 - Focal Shape Fixed Size: false - Focal Shape Size: 0.0500000007 + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.515398085 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6203979849815369 Target Frame: Value: Orbit (rviz) - Yaw: 0.490397513 + Yaw: 0.7453984618186951 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 846 + Height: 1312 Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000002a9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002a9000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002a9000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000022300fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000029d00000484fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000004840000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000484fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000004840000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000006c70000048400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: - collapsed: false - Width: 1200 - X: 59 - Y: 29 + collapsed: true + Width: 2416 + X: 144 + Y: 54 diff --git a/mir_description/rviz/mir_visu_full.rviz b/mir_description/rviz/mir_visu_full.rviz new file mode 100644 index 00000000..60710795 --- /dev/null +++ b/mir_description/rviz/mir_visu_full.rviz @@ -0,0 +1,393 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Map1/Topic1 + - /RobotModel1/Description Topic1 + - /TF1/Frames1 + - /Path1/Topic1 + - /Path2/Topic1 + Splitter Ratio: 0.5 + Tree Height: 820 + - 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 +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: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Angle Tolerance: 0.10000000149011612 + 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: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: false + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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/LaserScan + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 115; 210; 22 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.05000000074505806 + Name: Path + 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: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 117; 80; 123 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.019999999552965164 + Name: Path + 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: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Class: rviz_default_plugins/Pose + Color: 78; 154; 6 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + 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: 30.18552589416504 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -1.4263750314712524 + Y: 1.0514938831329346 + Z: 0.65448397397995 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5597963333129883 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.0003988649696111679 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001ea000003bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003bd000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003bd000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000047b000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0 diff --git a/mir_description/rviz/mir_visualization.rviz b/mir_description/rviz/mir_visualization.rviz new file mode 100644 index 00000000..9608d96a --- /dev/null +++ b/mir_description/rviz/mir_visualization.rviz @@ -0,0 +1,287 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /LaserScan1 + - /LaserScan1/Topic1 + - /RobotModel1 + - /RobotModel1/Description Topic1 + - /Map1 + - /Map1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 784 + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + back_laser_link: + Value: true + base_footprint: + Value: true + base_link: + Value: true + bl_caster_rotation_link: + Value: true + bl_caster_wheel_link: + Value: true + br_caster_rotation_link: + Value: true + br_caster_wheel_link: + Value: true + fl_caster_rotation_link: + Value: true + fl_caster_wheel_link: + Value: true + fr_caster_rotation_link: + Value: true + fr_caster_wheel_link: + Value: true + front_laser_link: + Value: true + imu_frame: + Value: true + imu_link: + Value: true + left_wheel_link: + Value: true + odom: + Value: true + right_wheel_link: + Value: true + surface: + Value: true + us_1_frame: + Value: true + us_2_frame: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + odom: + base_footprint: + base_link: + back_laser_link: + {} + bl_caster_rotation_link: + bl_caster_wheel_link: + {} + br_caster_rotation_link: + br_caster_wheel_link: + {} + fl_caster_rotation_link: + fl_caster_wheel_link: + {} + fr_caster_rotation_link: + fr_caster_wheel_link: + {} + front_laser_link: + {} + imu_link: + imu_frame: + {} + left_wheel_link: + {} + right_wheel_link: + {} + surface: + {} + us_1_frame: + {} + us_2_frame: + {} + Update Interval: 0 + 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/LaserScan + Color: 255; 0; 0 + 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: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.029999999329447746 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Best Effort + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 100 + Value: true + Visual Enabled: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: map_updates + Use Timestamp: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + 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 + 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: /move_base_simple/goal + - 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: 61.73799514770508 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -3.035649538040161 + Y: 2.1356096267700195 + Z: -25.291269302368164 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.45979642868042 + Target Frame: + Value: Orbit (rviz) + Yaw: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000002520000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003cb0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/mir_description/urdf/include/caster.urdf.xacro b/mir_description/urdf/include/caster.urdf.xacro new file mode 100644 index 00000000..58d97cd8 --- /dev/null +++ b/mir_description/urdf/include/caster.urdf.xacro @@ -0,0 +1,129 @@ + + + + + + + + + + + + + + + + + + ${ns} + + 200.0 + ${prefix}${locationprefix}_caster_rotation_joint + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + + + + + + + + + + + + + ${ns} + + 200.0 + ${prefix}${locationprefix}_caster_wheel_joint + + + + + + + + + + + + + + + + + + + + + + + Gazebo/DarkGrey + + + + + + + + + + 1.0 + 1.0 + 0 0 1 + 0.0 + 0.5 + + + + + + + 100000000.0 + + + 0.0 + + + + + + + diff --git a/mir_description/urdf/include/common.gazebo.xacro b/mir_description/urdf/include/common.gazebo.xacro index 87a13906..19f8df5b 100644 --- a/mir_description/urdf/include/common.gazebo.xacro +++ b/mir_description/urdf/include/common.gazebo.xacro @@ -1,15 +1,17 @@ - + - + + robot_description + robot_state_publisher - ${robot_namespace} + ${robot_namespace} - 0.001 - false + 0.001 + diff --git a/mir_description/urdf/include/common_properties.urdf.xacro b/mir_description/urdf/include/common_properties.urdf.xacro index 3c1a28df..0ec2f7d5 100644 --- a/mir_description/urdf/include/common_properties.urdf.xacro +++ b/mir_description/urdf/include/common_properties.urdf.xacro @@ -49,6 +49,11 @@ + + + + + diff --git a/mir_description/urdf/include/imu.gazebo.urdf.xacro b/mir_description/urdf/include/imu.gazebo.urdf.xacro index 08d91a2f..424972c4 100644 --- a/mir_description/urdf/include/imu.gazebo.urdf.xacro +++ b/mir_description/urdf/include/imu.gazebo.urdf.xacro @@ -16,19 +16,70 @@ true true - 100 + ${update_rate} true - __default_topic__ + + + + + 0.0 + ${stdev} + 0.0000075 + 0.0000008 + + + + + 0.0 + ${stdev} + 0.0000075 + 0.0000008 + + + + + 0.0 + ${stdev} + 0.0000075 + 0.0000008 + + + + + + + 0.0 + ${stdev} + 0.1 + 0.001 + + + + + 0.0 + ${stdev} + 0.1 + 0.001 + + + + + 0.0 + ${stdev} + 0.1 + 0.001 + + + + - ${imu_topic} - ${link} - ${update_rate} - ${stdev * stdev} - 0 0 0 - 0 0 0 - ${imu_frame} + false + + ${imu_topic} + ~/out:=data + ${imu_frame} + - 0 0 0 0 0 0 diff --git a/mir_description/urdf/include/mir_100.gazebo.xacro b/mir_description/urdf/include/mir_100.gazebo.xacro index 5293163e..2b625d9c 100644 --- a/mir_description/urdf/include/mir_100.gazebo.xacro +++ b/mir_description/urdf/include/mir_100.gazebo.xacro @@ -1,61 +1,61 @@ - + + - - false - true - 1000.0 - ${left_wheel_joint} - ${right_wheel_joint} - ${wheel_separation} - ${2*wheel_radius} - 10 - 1 - map - mobile_base_controller/cmd_vel - mobile_base_controller/odom - base_footprint - 2.8 - true - false - world - Debug + + + + ${ns} + + 1000.0 + ${right_wheel_joint} + ${left_wheel_joint} - - - - - - - - - 0.01 - - + + + cmd_vel:=${prefix}cmd_vel + odom:=${prefix}odom + - - - - - - - + false + true + + 1000.0 + ${left_wheel_joint} + ${right_wheel_joint} + ${wheel_separation} + ${2*wheel_radius} + ${prefix}odom + + ${prefix}base_footprint + true + false + true + + 10 + 2.8 + + + + - true - 50.0 - ${prefix}base_footprint - base_pose_ground_truth - 0.01 - map - 0 0 0 - 0 0 0 + + odom:=${prefix}base_pose_ground_truth + + true + 50.0 + ${prefix}base_footprint + 0.01 + map + 0 0 0 + 0 0 0 diff --git a/mir_description/urdf/include/mir_100_v1.urdf.xacro b/mir_description/urdf/include/mir_100_v1.urdf.xacro index e0bed0a4..c22a8e48 100644 --- a/mir_description/urdf/include/mir_100_v1.urdf.xacro +++ b/mir_description/urdf/include/mir_100_v1.urdf.xacro @@ -1,14 +1,25 @@ + + + - + + + + + + + @@ -27,113 +38,15 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/DarkGrey - - - - - - - - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/DarkGrey - - + - + @@ -143,8 +56,8 @@ - - + + @@ -173,7 +86,7 @@ - + + + + + + + + + + @@ -222,10 +144,10 @@ - - - - + + + + @@ -238,9 +160,6 @@ - - - - + diff --git a/mir_description/urdf/include/sick_s300.urdf.xacro b/mir_description/urdf/include/sick_s300.urdf.xacro index a2fcca77..3b846613 100644 --- a/mir_description/urdf/include/sick_s300.urdf.xacro +++ b/mir_description/urdf/include/sick_s300.urdf.xacro @@ -32,9 +32,7 @@ - - 0 0 0 0 0 0 false 12.5 @@ -53,16 +51,19 @@ gaussian - 0.0 0.01 - - ${prefix}${link} - ${topic} + + + ~/out:=${prefix}${topic} + + sensor_msgs/LaserScan + ${prefix}${link} diff --git a/mir_description/urdf/include/wheel.urdf.xacro b/mir_description/urdf/include/wheel.urdf.xacro new file mode 100644 index 00000000..404690ba --- /dev/null +++ b/mir_description/urdf/include/wheel.urdf.xacro @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/DarkGrey + + + + + + + + + + 300.0 + 300.0 + 0 0 1 + + + + 300 + false + ${mir_100_act_wheel_radius} + + + + + + + 0.001 + + + + + + + diff --git a/mir_description/urdf/mir.urdf.xacro b/mir_description/urdf/mir.urdf.xacro index c7a0c0b4..15fbe22d 100644 --- a/mir_description/urdf/mir.urdf.xacro +++ b/mir_description/urdf/mir.urdf.xacro @@ -5,24 +5,24 @@ + - - + + + + - - + + - - + - + diff --git a/mir_driver/CMakeLists.txt b/mir_driver/CMakeLists.txt deleted file mode 100644 index 8862ab21..00000000 --- a/mir_driver/CMakeLists.txt +++ /dev/null @@ -1,68 +0,0 @@ -cmake_minimum_required(VERSION 3.5.1) -cmake_policy(SET CMP0048 NEW) -project(mir_driver) - -find_package(catkin REQUIRED COMPONENTS - actionlib_msgs - diagnostic_msgs - dynamic_reconfigure - geometry_msgs - mir_actions - mir_msgs - move_base_msgs - nav_msgs - rosgraph_msgs - roslaunch - rospy - rospy_message_converter - sensor_msgs - std_msgs - tf - visualization_msgs -) - -catkin_python_setup() - -################################### -## catkin specific configuration ## -################################### -catkin_package( - CATKIN_DEPENDS - actionlib_msgs - diagnostic_msgs - dynamic_reconfigure - geometry_msgs - mir_actions - mir_msgs - move_base_msgs - nav_msgs - rosgraph_msgs - rospy_message_converter - sensor_msgs - std_msgs - tf - visualization_msgs -) - -############# -## Install ## -############# - -catkin_install_python(PROGRAMS - nodes/fake_mir_joint_publisher.py - nodes/mir_bridge.py - nodes/rep117_filter.py - nodes/tf_remove_child_frames.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# - -roslaunch_add_file_check(launch) diff --git a/mir_driver/launch/additions_launch.py b/mir_driver/launch/additions_launch.py new file mode 100644 index 00000000..c32e4f74 --- /dev/null +++ b/mir_driver/launch/additions_launch.py @@ -0,0 +1,44 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + + mir_description_dir = get_package_share_directory('mir_description') + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time', default=False) + rviz_config_file = LaunchConfiguration('rviz_config_file') + + return LaunchDescription([ + + DeclareLaunchArgument( + name='namespace', + default_value='' + ), + + DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join(mir_description_dir, 'rviz', 'mir_visu_full.rviz'), + description='Define rviz config to be used' + ), + + Node( + package='teleop_twist_keyboard', + executable='teleop_twist_keyboard', + namespace=namespace, + prefix='xterm -e'), + + Node( + package='rviz2', + executable='rviz2', + output={'both': 'log'}, + parameters=[{'use_sim_time': use_sim_time}], + arguments=['-d', rviz_config_file] + ), + + ]) diff --git a/mir_driver/launch/mir.launch b/mir_driver/launch/mir.launch deleted file mode 100644 index 70d8524b..00000000 --- a/mir_driver/launch/mir.launch +++ /dev/null @@ -1,81 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - base_link - - front_laser_link - - back_laser_link - - camera_top_link - - camera_top_depth_optical_frame - - camera_floor_link - - camera_floor_depth_optical_frame - - imu_link - - - - - - - - - - - - - - - - - - - - - - odom - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mir_driver/launch/mir_headless_launch.py b/mir_driver/launch/mir_headless_launch.py new file mode 100644 index 00000000..72042659 --- /dev/null +++ b/mir_driver/launch/mir_headless_launch.py @@ -0,0 +1,104 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + + mir_description_dir = get_package_share_directory('mir_description') + use_sim_time = LaunchConfiguration('use_sim_time') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'namespace', + default_value='', + description='Namespace to push all topics into.'), + + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description=''), + + DeclareLaunchArgument( + 'mir_hostname', + default_value='192.168.12.20', + description=''), + + # DeclareLaunchArgument( + # 'disable_map', + # default_value='false', + # description='Disable the map topic and map -> odom_comb TF transform from the MiR'), + + DeclareLaunchArgument( + 'robot_state_publisher_enabled', + default_value='true', + description='Set to true to publish tf using mir_description'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_description_dir, 'launch', 'mir_launch.py')), + launch_arguments={ + 'joint_state_publisher_enabled': 'false', + 'namespace': LaunchConfiguration('namespace') + }.items(), + condition=IfCondition(LaunchConfiguration( + 'robot_state_publisher_enabled')) + ), + + Node( + package='mir_driver', + executable='mir_bridge', + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time'), + 'tf_prefix': LaunchConfiguration('namespace')}], + namespace=LaunchConfiguration('namespace'), + output='screen'), + + Node( + package='mir_driver', + executable='fake_mir_joint_publisher', + remappings=[('use_sim_time', LaunchConfiguration('use_sim_time'))], + parameters=[{'tf_prefix': LaunchConfiguration('namespace')}], + namespace=LaunchConfiguration('namespace'), + output='screen'), + + Node( + package='twist_stamper', + executable='twist_stamper', + name='twist_stamper_cmd_vel_mir', + parameters=[ + {'use_sim_time': LaunchConfiguration('use_sim_time')} + ], + remappings=[ + ('cmd_vel_in', 'cmd_vel'), + ('cmd_vel_out', 'cmd_vel_stamped'), + ], + namespace=LaunchConfiguration('namespace'), + ), + + Node( + package='ira_laser_tools', + name='mir_laser_scan_merger', + executable='laserscan_multi_merger', + parameters=[{'laserscan_topics': "b_scan f_scan", + 'destination_frame': "virtual_laser_link", + 'scan_destination_topic': 'scan', + 'cloud_destination_topic': 'scan_cloud', + 'min_height': -0.25, + 'max_merge_time_diff': 0.05, + # driver (msg converter) delay + 'max_delay_scan_time': 2.5, + 'max_completion_time': 0.1, + 'alow_scan_delay': True, + 'use_sim_time': use_sim_time, + 'best_effort': False}], + namespace=LaunchConfiguration('namespace'), + output='screen') + + ]) diff --git a/mir_driver/launch/mir_launch.py b/mir_driver/launch/mir_launch.py new file mode 100644 index 00000000..5169f22a --- /dev/null +++ b/mir_driver/launch/mir_launch.py @@ -0,0 +1,25 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + + mir_driver = get_package_share_directory('mir_driver') + + return LaunchDescription([ + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_driver, 'launch', 'mir_headless_launch.py')), + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_driver, 'launch', 'additions_launch.py')), + ) + + ]) diff --git a/mir_driver/mir_driver/__init__.py b/mir_driver/mir_driver/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mir_driver/mir_driver/fake_mir_joint_publisher.py b/mir_driver/mir_driver/fake_mir_joint_publisher.py new file mode 100755 index 00000000..2347e1a0 --- /dev/null +++ b/mir_driver/mir_driver/fake_mir_joint_publisher.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +import time +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState +from rclpy.qos import qos_profile_system_default + + +class fake_mir_joint_publisher(Node): + + def __init__(self): + super().__init__('fake_mir_joint_publisher') + + self.declare_parameter('tf_prefix', '') + tf_prefix = self.get_parameter('tf_prefix').get_parameter_value().string_value.strip('/') + if tf_prefix != "": + tf_prefix = tf_prefix + '/' + + pub = self.create_publisher( + msg_type=JointState, + topic='joint_states', # no prefix to joint states, just namespace + qos_profile=qos_profile_system_default # TODO Check QoS Settings + ) + + pub_rate = 1 + + while(rclpy.ok()): + js = JointState() + js.header.stamp = self.get_clock().now().to_msg() + js.name = [tf_prefix + 'left_wheel_joint', tf_prefix + 'right_wheel_joint', + tf_prefix + 'fl_caster_rotation_joint', tf_prefix + 'fl_caster_wheel_joint', + tf_prefix + 'fr_caster_rotation_joint', tf_prefix + 'fr_caster_wheel_joint', + tf_prefix + 'bl_caster_rotation_joint', tf_prefix + 'bl_caster_wheel_joint', + tf_prefix + 'br_caster_rotation_joint', tf_prefix + 'br_caster_wheel_joint'] + js.position = [0.0 for _ in js.name] + js.velocity = [0.0 for _ in js.name] + js.effort = [0.0 for _ in js.name] + pub.publish(js) + time.sleep(pub_rate) + + +def main(args=None): + rclpy.init(args=args) + node = fake_mir_joint_publisher() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/mir_driver/mir_driver/mir_bridge.py b/mir_driver/mir_driver/mir_bridge.py new file mode 100755 index 00000000..acf65fb7 --- /dev/null +++ b/mir_driver/mir_driver/mir_bridge.py @@ -0,0 +1,510 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_system_default, qos_profile_sensor_data + +import time +import copy +import sys +from collections.abc import Iterable + +import mir_driver.rosbridge +from rclpy_message_converter import message_converter +from geometry_msgs.msg import TwistStamped +from nav_msgs.msg import Odometry +from sensor_msgs.msg import LaserScan +from tf2_msgs.msg import TFMessage +from std_srvs.srv import Trigger + +tf_prefix = '' + + +class TopicConfig(object): + def __init__(self, topic, topic_type, topic_renamed=None, latch=False, dict_filter=None, + qos_profile=None): + self.topic = topic + if (topic_renamed): + self.topic_ros2_name = topic_renamed + else: + self.topic_ros2_name = topic + self.topic_type = topic_type + self.latch = latch + self.dict_filter = dict_filter + if qos_profile is not None: + self.qos_profile = qos_profile + else: + self.qos_profile = qos_profile_system_default + + +def _odom_dict_filter(msg_dict, to_ros2): + filtered_msg_dict = copy.deepcopy(msg_dict) + filtered_msg_dict['header'] = _convert_ros_header(filtered_msg_dict['header'], to_ros2) + filtered_msg_dict['child_frame_id'] = tf_prefix + \ + filtered_msg_dict['child_frame_id'].strip('/') + return filtered_msg_dict + + +def _tf_dict_filter(msg_dict, to_ros2): + filtered_msg_dict = copy.deepcopy(msg_dict) + + for transform in filtered_msg_dict['transforms']: + transform['child_frame_id'] = tf_prefix + transform['child_frame_id'].strip('/') + transform['header'] = _convert_ros_header(transform['header'], to_ros2) + return filtered_msg_dict + + +def _laser_scan_filter(msg_dict, to_ros2): + filtered_msg_dict = copy.deepcopy(msg_dict) + filtered_msg_dict['header'] = _convert_ros_header( + filtered_msg_dict['header'], to_ros2) + return filtered_msg_dict + + +def _map_dict_filter(msg_dict, to_ros2): + filtered_msg_dict = copy.deepcopy(msg_dict) + filtered_msg_dict['header'] = _convert_ros_header( + filtered_msg_dict['header'], to_ros2) + filtered_msg_dict['info']['map_load_time'] = _convert_ros_time( + filtered_msg_dict['info']['map_load_time'], to_ros2) + print('called dict filter') + return filtered_msg_dict + + +def _convert_ros_time(time_msg_dict, to_ros2): + time_dict = copy.deepcopy(time_msg_dict) + if to_ros2: + # Conversion from MiR (ros1) to sys (ros2) + time_dict['nanosec'] = time_dict.pop('nsecs') + time_dict['sec'] = time_dict.pop('secs') + else: + # Conversion from sys (ros2) to MiR (ros1) + time_dict['nsecs'] = time_dict.pop('nanosec') + time_dict['secs'] = time_dict.pop('sec') + + return time_dict + + +def _convert_ros_header(header_msg_dict, to_ros2): + header_dict = copy.deepcopy(header_msg_dict) + header_dict['stamp'] = _convert_ros_time(header_dict['stamp'], to_ros2) + if to_ros2: + del header_dict['seq'] + frame_id = header_dict['frame_id'].strip('/') + header_dict['frame_id'] = tf_prefix + frame_id + else: # to ros1 + header_dict['seq'] = 0 + # remove tf_prefix to frame_id + + return header_dict + + +def _prepend_tf_prefix_dict_filter(msg_dict): + # filtered_msg_dict = copy.deepcopy(msg_dict) + if not isinstance(msg_dict, dict): # can happen during recursion + return + for (key, value) in msg_dict.items(): + if key == 'header': + try: + # prepend frame_id + frame_id = value['frame_id'].strip('/') + if (frame_id != 'map'): + # prepend tf_prefix, then remove leading '/' (e.g., when tf_prefix is empty) + value['frame_id'] = frame_id.strip('/') + else: + value['frame_id'] = frame_id + + except TypeError: + pass # value is not a dict + except KeyError: + pass # value doesn't have key 'frame_id' + elif isinstance(value, dict): + _prepend_tf_prefix_dict_filter(value) + elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list) + for item in value: + _prepend_tf_prefix_dict_filter(item) + return msg_dict + + +def _remove_tf_prefix_dict_filter(msg_dict): + # filtered_msg_dict = copy.deepcopy(msg_dict) + if not isinstance(msg_dict, dict): # can happen during recursion + return + for (key, value) in msg_dict.items(): + if key == 'header': + try: + # remove frame_id + s = value['frame_id'].strip('/') + if s.find(tf_prefix) == 0: + # strip off tf_prefix, then strip leading '/' + value['frame_id'] = (s[len(tf_prefix):]).strip('/') + except TypeError: + pass # value is not a dict + except KeyError: + pass # value doesn't have key 'frame_id' + elif isinstance(value, dict): + _remove_tf_prefix_dict_filter(value) + elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list) + for item in value: + _remove_tf_prefix_dict_filter(item) + return msg_dict + + +# topics we want to publish to ROS (and subscribe to from the MiR) +PUB_TOPICS = [ + # TopicConfig('LightCtrl/bms_data', mir_msgs.msg.BMSData), + # TopicConfig('LightCtrl/charging_state', mir_msgs.msg.ChargingState), + # TopicConfig('LightCtrl/us_list', sensor_msgs.msg.Range), + # TopicConfig('MC/battery_currents', mir_msgs.msg.BatteryCurrents), + # TopicConfig('MC/battery_voltage', std_msgs.msg.Float64), + # TopicConfig('MC/currents', sdc21x0.msg.MotorCurrents), + # TopicConfig('MC/encoders', sdc21x0.msg.StampedEncoders), + # TopicConfig('MissionController/CheckArea/visualization_marker', + # visualization_msgs.msg.Marker), + # TopicConfig('MissionController/goal_position_guid', std_msgs.msg.String), + # TopicConfig('MissionController/prompt_user', mir_msgs.msg.UserPrompt), + # TopicConfig('SickPLC/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('SickPLC/parameter_updates', dynamic_reconfigure.msg.Config), + # TopicConfig('active_mapping_guid', std_msgs.msg.String), + # TopicConfig('amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped), + # TopicConfig('b_raw_scan', sensor_msgs.msg.LaserScan), + TopicConfig('b_scan', LaserScan, dict_filter=_laser_scan_filter, + qos_profile=qos_profile_sensor_data), + # TopicConfig('camera_floor/background', sensor_msgs.msg.PointCloud2), + # TopicConfig('camera_floor/depth/parameter_descriptions', + # dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('camera_floor/depth/parameter_updates', dynamic_reconfigure.msg.Config), + # TopicConfig('camera_floor/depth/points', sensor_msgs.msg.PointCloud2), + # TopicConfig('camera_floor/filter/visualization_marker', visualization_msgs.msg.Marker), + # TopicConfig('camera_floor/floor', sensor_msgs.msg.PointCloud2), + # TopicConfig('camera_floor/obstacles', sensor_msgs.msg.PointCloud2), + # TopicConfig('check_area/polygon', geometry_msgs.msg.PolygonStamped), + # TopicConfig('check_pose_area/polygon', geometry_msgs.msg.PolygonStamped), + # TopicConfig('data_events/area_events', mir_data_msgs.msg.AreaEventEvent), + # TopicConfig('data_events/maps', mir_data_msgs.msg.MapEvent), + # TopicConfig('data_events/positions', mir_data_msgs.msg.PositionEvent), + # TopicConfig('data_events/registers', mir_data_msgs.msg.PLCRegisterEvent), + # TopicConfig('data_events/sounds', mir_data_msgs.msg.SoundEvent), + # TopicConfig('diagnostics', diagnostic_msgs.msg.DiagnosticArray), + # TopicConfig('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray), + # TopicConfig('diagnostics_toplevel_state', diagnostic_msgs.msg.DiagnosticStatus), + # TopicConfig('f_raw_scan', sensor_msgs.msg.LaserScan), + TopicConfig('f_scan', LaserScan, dict_filter=_laser_scan_filter, + qos_profile=qos_profile_sensor_data), + # TopicConfig('imu_data', sensor_msgs.msg.Imu), + # TopicConfig('laser_back/driver/parameter_descriptions', + # dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('laser_back/driver/parameter_updates', dynamic_reconfigure.msg.Config), + # TopicConfig('laser_front/driver/parameter_descriptions', + # dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('laser_front/driver/parameter_updates', dynamic_reconfigure.msg.Config), + # TopicConfig('localization_score', std_msgs.msg.Float64), + # TopicConfig('/map', nav_msgs.msg.OccupancyGrid, latch=True), + # TopicConfig('/map_metadata', nav_msgs.msg.MapMetaData), + # TopicConfig('marker_tracking_node/feedback', + # mir_marker_tracking.msg.MarkerTrackingActionFeedback), + # TopicConfig('marker_tracking_node/laser_line_extract/parameter_descriptions', + # dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('marker_tracking_node/laser_line_extract/parameter_updates', + # dynamic_reconfigure.msg.Config), + # TopicConfig('marker_tracking_node/laser_line_extract/visualization_marker', + # visualization_msgs.msg.Marker), + # TopicConfig('marker_tracking_node/result', + # mir_marker_tracking.msg.MarkerTrackingActionResult), + # TopicConfig('marker_tracking_node/status', actionlib_msgs.msg.GoalStatusArray), + # TopicConfig('mirEventTrigger/events', mir_msgs.msg.Events), + # TopicConfig('mir_amcl/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('mir_amcl/parameter_updates', dynamic_reconfigure.msg.Config), + # TopicConfig('mir_amcl/selected_points', sensor_msgs.msg.PointCloud2), + # TopicConfig('mir_log', rosgraph_msgs.msg.Log), + # TopicConfig('mir_sound/sound_event', mir_msgs.msg.SoundEvent), + # TopicConfig('mir_status_msg', std_msgs.msg.String), + # TopicConfig('mirspawn/node_events', mirSpawn.msg.LaunchItem), + # TopicConfig('mirwebapp/grid_map_metadata', mir_msgs.msg.LocalMapStat), + # TopicConfig('mirwebapp/laser_map_metadata', mir_msgs.msg.LocalMapStat), + # TopicConfig('mirwebapp/web_path', mir_msgs.msg.WebPath), + # really mir_actions/MirMoveBaseActionFeedback: + # TopicConfig('move_base/feedback', move_base_msgs.msg.MoveBaseActionFeedback, + # dict_filter=_move_base_feedback_dict_filter), + # really mir_actions/MirMoveBaseActionResult: + # TopicConfig('move_base/result', move_base_msgs.msg.MoveBaseActionResult, + # dict_filter=_move_base_result_dict_filter), + # TopicConfig('move_base/status', actionlib_msgs.msg.GoalStatusArray), + # TopicConfig('move_base_node/MIRPlannerROS/cost_cloud', sensor_msgs.msg.PointCloud2), + # TopicConfig('move_base_node/MIRPlannerROS/global_plan', nav_msgs.msg.Path), + # TopicConfig('move_base_node/MIRPlannerROS/len_to_goal', std_msgs.msg.Float64), + # TopicConfig('move_base_node/MIRPlannerROS/local_plan', nav_msgs.msg.Path), + # TopicConfig('move_base_node/MIRPlannerROS/parameter_descriptions', + # dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('move_base_node/MIRPlannerROS/parameter_updates', + # dynamic_reconfigure.msg.Config), + # TopicConfig('move_base_node/MIRPlannerROS/updated_global_plan', mir_msgs.msg.PlanSegments), + # TopicConfig('move_base_node/MIRPlannerROS/visualization_marker', + # visualization_msgs.msg.MarkerArray), + # TopicConfig('move_base_node/SBPLLatticePlanner/plan', nav_msgs.msg.Path), + # TopicConfig('move_base_node/SBPLLatticePlanner/sbpl_lattice_planner_stats', + # sbpl_lattice_planner.msg.SBPLLatticePlannerStats), + # TopicConfig('move_base_node/SBPLLatticePlanner/visualization_marker', + # visualization_msgs.msg.MarkerArray), + # TopicConfig('move_base_node/current_goal', geometry_msgs.msg.PoseStamped), + # TopicConfig('move_base_node/global_costmap/inflated_obstacles', nav_msgs.msg.GridCells), + # TopicConfig('move_base_node/global_costmap/obstacles', nav_msgs.msg.GridCells), + # TopicConfig('move_base_node/global_costmap/parameter_descriptions', + # dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('move_base_node/global_costmap/parameter_updates', + # dynamic_reconfigure.msg.Config), + # TopicConfig('move_base_node/global_costmap/robot_footprint', + # geometry_msgs.msg.PolygonStamped), + # TopicConfig('move_base_node/global_costmap/unknown_space', nav_msgs.msg.GridCells), + # TopicConfig('move_base_node/global_plan', nav_msgs.msg.Path), + # TopicConfig('move_base_node/local_costmap/inflated_obstacles', nav_msgs.msg.GridCells), + # TopicConfig('move_base_node/local_costmap/obstacles', nav_msgs.msg.GridCells), + # TopicConfig('move_base_node/local_costmap/parameter_descriptions', + # dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('move_base_node/local_costmap/parameter_updates', + # dynamic_reconfigure.msg.Config), + # TopicConfig('move_base_node/local_costmap/robot_footprint', + # geometry_msgs.msg.PolygonStamped), + # TopicConfig('move_base_node/local_costmap/safety_zone', geometry_msgs.msg.PolygonStamped), + # TopicConfig('move_base_node/local_costmap/unknown_space', nav_msgs.msg.GridCells), + # TopicConfig('move_base_node/mir_escape_recovery/visualization_marker', + # visualization_msgs.msg.Marker), + # TopicConfig('move_base_node/parameter_descriptions', + # dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('move_base_node/parameter_updates', dynamic_reconfigure.msg.Config), + # TopicConfig('move_base_node/time_to_coll', std_msgs.msg.Float64), + # TopicConfig('move_base_node/traffic_costmap/inflated_obstacles', nav_msgs.msg.GridCells), + # TopicConfig('move_base_node/traffic_costmap/obstacles', nav_msgs.msg.GridCells), + # TopicConfig('move_base_node/traffic_costmap/parameter_descriptions', + # dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('move_base_node/traffic_costmap/parameter_updates', + # dynamic_reconfigure.msg.Config), + # TopicConfig('move_base_node/traffic_costmap/robot_footprint', + # geometry_msgs.msg.PolygonStamped), + # TopicConfig('move_base_node/traffic_costmap/unknown_space', nav_msgs.msg.GridCells), + # TopicConfig('move_base_node/visualization_marker', visualization_msgs.msg.Marker), + # TopicConfig('move_base_simple/visualization_marker', visualization_msgs.msg.Marker), + TopicConfig('odom', Odometry, dict_filter=_odom_dict_filter), + # TopicConfig('odom_enc', nav_msgs.msg.Odometry), + # TopicConfig('one_way_map', nav_msgs.msg.OccupancyGrid), + # TopicConfig('param_update', std_msgs.msg.String), + # TopicConfig('particlevizmarker', visualization_msgs.msg.MarkerArray), + # TopicConfig('resource_tracker/needed_resources', mir_msgs.msg.ResourcesState), + # TopicConfig('robot_mode', mir_msgs.msg.RobotMode), + # TopicConfig('robot_pose', geometry_msgs.msg.Pose), + # TopicConfig('robot_state', mir_msgs.msg.RobotState), + # TopicConfig('robot_status', mir_msgs.msg.RobotStatus), + # TopicConfig('/rosout', rosgraph_msgs.msg.Log), + # TopicConfig('/rosout_agg', rosgraph_msgs.msg.Log), + # TopicConfig('scan', sensor_msgs.msg.LaserScan), + # TopicConfig('scan_filter/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), + # TopicConfig('scan_filter/parameter_updates', dynamic_reconfigure.msg.Config), + # TopicConfig('scan_filter/visualization_marker', visualization_msgs.msg.Marker), + # TopicConfig('session_importer_node/info', mirSessionImporter.msg.SessionImportInfo), + # TopicConfig('set_mc_PID', std_msgs.msg.Float64MultiArray), + # let /tf be /tf if namespaced + TopicConfig('tf', TFMessage, dict_filter=_tf_dict_filter, topic_renamed='/tf'), + # TopicConfig('/tf_static', tf2_msgs.msg.TFMessage, dict_filter=_tf_static_dict_filter, + # latch=True), + # TopicConfig('traffic_map', nav_msgs.msg.OccupancyGrid), + # TopicConfig('wifi_diagnostics', diagnostic_msgs.msg.DiagnosticArray), + # TopicConfig('wifi_diagnostics/cur_ap', mir_wifi_msgs.msg.APInfo), + # TopicConfig('wifi_diagnostics/roam_events', mir_wifi_msgs.msg.WifiRoamEvent), + # TopicConfig('wifi_diagnostics/wifi_ap_interface_stats', + # mir_wifi_msgs.msg.WifiInterfaceStats), + # TopicConfig('wifi_diagnostics/wifi_ap_rssi', mir_wifi_msgs.msg.APRssiStats), + # TopicConfig('wifi_diagnostics/wifi_ap_time_stats', mir_wifi_msgs.msg.APTimeStats), + # TopicConfig('wifi_watchdog/ping', mir_wifi_msgs.msg.APPingStats), +] + +# topics we want to subscribe to from ROS (and publish to the MiR) +SUB_TOPICS = [ + TopicConfig('cmd_vel', TwistStamped, 'cmd_vel_stamped') + # TopicConfig('initialpose', geometry_msgs.msg.PoseWithCovarianceStamped), + # TopicConfig('light_cmd', std_msgs.msg.String), + # TopicConfig('mir_cmd', std_msgs.msg.String), + # TopicConfig('move_base/cancel', actionlib_msgs.msg.GoalID), + # really mir_actions/MirMoveBaseActionGoal: + # TopicConfig('move_base/goal', move_base_msgs.msg.MoveBaseActionGoal, + # dict_filter=_move_base_goal_dict_filter), +] + + +class PublisherWrapper(object): + def __init__(self, topic_config, nh): + self.topic_config = topic_config + self.robot = nh.robot + self.connected = False + self.sub = nh.create_subscription( + msg_type=topic_config.topic_type, + topic=topic_config.topic, + callback=self.callback, + qos_profile=topic_config.qos_profile + ) + self.pub = nh.create_publisher( + msg_type=topic_config.topic_type, + topic=topic_config.topic_ros2_name, + qos_profile=topic_config.qos_profile + ) + + nh.get_logger().info("Publishing topic '%s' [%s]" % + (topic_config.topic_ros2_name, topic_config.topic_type.__module__)) + # latched topics must be subscribed immediately + # if topic_config.latch: + self.peer_subscribe(None, None, None, nh) + + def peer_subscribe(self, topic_name, topic_publish, peer_publish, nh): + if not self.connected: + self.connected = True + nh.get_logger().info("Starting to stream messages on topic '%s'" % + self.topic_config.topic) + self.robot.subscribe( + topic=('/' + self.topic_config.topic), callback=self.callback) + + def callback(self, msg_dict): + if not isinstance(msg_dict, dict): # can happen during recursion + return + msg_dict = _prepend_tf_prefix_dict_filter(msg_dict) + if self.topic_config.dict_filter is not None: + msg_dict = self.topic_config.dict_filter(msg_dict, to_ros2=True) + msg = message_converter.convert_dictionary_to_ros_message( + self.topic_config.topic_type, msg_dict) + self.pub.publish(msg) + + +class SubscriberWrapper(object): + def __init__(self, topic_config, nh): + self.topic_config = topic_config + self.robot = nh.robot + self.sub = nh.create_subscription( + msg_type=topic_config.topic_type, + topic=topic_config.topic_ros2_name, + callback=self.callback, + qos_profile=topic_config.qos_profile + ) + + nh.get_logger().info("Subscribing to topic '%s' [%s]" % ( + topic_config.topic, topic_config.topic_type.__module__)) + + def callback(self, msg): + if msg is None: + return + + msg_dict = message_converter.convert_ros_message_to_dictionary(msg) + msg_dict = _remove_tf_prefix_dict_filter(msg_dict) + + if self.topic_config.dict_filter is not None: + msg_dict = self.topic_config.dict_filter(msg_dict, to_ros2=False) + self.robot.publish('/' + self.topic_config.topic, msg_dict) + + +class MiR100BridgeNode(Node): + def __init__(self): + super().__init__('mir_bridge') + + self.mir_bridge_ready = False # state + self.srv_mir_ready = self.create_service( + Trigger, 'mir_bridge_ready', self.mir_bridge_ready_poll_callback) + + try: + hostname = self.declare_parameter( + 'hostname', '192.168.12.20').value + except KeyError: + self.get_logger().fatal('Parameter "hostname" is not set!') + sys.exit(-1) + port = self.declare_parameter('port', 9090).value + assert isinstance(port, int), 'port parameter must be an integer' + + global tf_prefix + self.declare_parameter('tf_prefix', '') + tf_prefix = self.get_parameter('tf_prefix').get_parameter_value().string_value.strip('/') + if tf_prefix != "": + tf_prefix = tf_prefix + '/' + + self.get_logger().info('Trying to connect to %s:%i...' % (hostname, port)) + self.robot = mir_driver.rosbridge.RosbridgeSetup(hostname, port) + + i = 1 + while not self.robot.is_connected(): + if not rclpy.ok(): + sys.exit(0) + if self.robot.is_errored(): + self.get_logger().fatal('Connection error to %s:%i, giving up!' + % (hostname, port)) + sys.exit(-1) + if i % 10 == 0: + self.get_logger().warn('Still waiting for connection to %s:%i...' + % (hostname, port)) + i += 1 + time.sleep(1) + self.get_logger().info('Connected to %s:%i...' % (hostname, port)) + + topics = self.get_topics() + published_topics = [topic_name for (topic_name, _, has_publishers, _) + in topics if has_publishers] + subscribed_topics = [topic_name for (topic_name, _, _, has_subscribers) + in topics if has_subscribers] + + for pub_topic in PUB_TOPICS: + PublisherWrapper(pub_topic, self) + if ('/' + pub_topic.topic) not in published_topics: + self.get_logger().warn("Topic '%s' is not published by the MiR!" % pub_topic.topic) + + for sub_topic in SUB_TOPICS: + SubscriberWrapper(sub_topic, self) + if ('/' + sub_topic.topic) not in subscribed_topics: + self.get_logger().warn( + "Topic '%s' is not yet subscribed to by the MiR!" % sub_topic.topic) + + self.mir_bridge_ready = True + + def get_topics(self): + srv_response = self.robot.callService('/rosapi/topics', msg={}) + topic_names = sorted(srv_response['topics']) + topics = [] + + for topic_name in topic_names: + srv_response = self.robot.callService( + "/rosapi/topic_type", msg={'topic': topic_name}) + topic_type = srv_response['type'] + + srv_response = self.robot.callService( + "/rosapi/publishers", msg={'topic': topic_name}) + has_publishers = True if len( + srv_response['publishers']) > 0 else False + + srv_response = self.robot.callService( + "/rosapi/subscribers", msg={'topic': topic_name}) + has_subscribers = True if len( + srv_response['subscribers']) > 0 else False + + topics.append( + [topic_name, topic_type, has_publishers, has_subscribers]) + + print('Publishers:') + for (topic_name, topic_type, has_publishers, has_subscribers) in topics: + if has_publishers: + print((' * %s [%s]' % (topic_name, topic_type))) + + print('\nSubscribers:') + for (topic_name, topic_type, has_publishers, has_subscribers) in topics: + if has_subscribers: + print((' * %s [%s]' % (topic_name, topic_type))) + + return topics + + def mir_bridge_ready_poll_callback(self, request, response): + self.get_logger().info('Checked for readiness') + response.success = self.mir_bridge_ready + response.message = "" + return response + + +def main(args=None): + rclpy.init(args=args) + node = MiR100BridgeNode() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/mir_driver/src/mir_driver/rosbridge.py b/mir_driver/mir_driver/rosbridge.py similarity index 86% rename from mir_driver/src/mir_driver/rosbridge.py rename to mir_driver/mir_driver/rosbridge.py index e5e9801e..9f54e460 100644 --- a/mir_driver/src/mir_driver/rosbridge.py +++ b/mir_driver/mir_driver/rosbridge.py @@ -9,7 +9,7 @@ import random -class RosbridgeSetup: +class RosbridgeSetup(): def __init__(self, host, port): self.callbacks = {} self.service_callbacks = {} @@ -50,10 +50,10 @@ def unsubscribe(self, topic): def callService(self, serviceName, callback=None, msg=None): id = self.generate_id() call = {"op": "call_service", "id": id, "service": serviceName} - if msg != None: + if msg is not None: call['args'] = msg - if callback == None: + if callback is None: self.resp = None def internalCB(msg): @@ -63,7 +63,7 @@ def internalCB(msg): self.addServiceCallback(id, internalCB) self.send(call) - while self.resp == None: + while self.resp is None: time.sleep(0.01) return self.resp @@ -75,12 +75,13 @@ def internalCB(msg): def send(self, obj): try: self.connection.sendString(json.dumps(obj)) - except: + except Exception: traceback.print_exc() raise def generate_id(self, chars=16): - return ''.join(random.SystemRandom().choice(string.ascii_letters + string.digits) for _ in range(chars)) + return ''.join(random.SystemRandom().choice( + string.ascii_letters + string.digits) for _ in range(chars)) def addServiceCallback(self, id, callback): self.service_callbacks[id] = callback @@ -114,8 +115,9 @@ def onMessageReceived(self, message): for callback in self.callbacks[topic]: try: callback(msg) - except: - print("exception on callback", callback, "from", topic) + except Exception: + print("exception on callback", + callback, "from", topic) traceback.print_exc() raise elif option == "service_response": @@ -126,7 +128,7 @@ def onMessageReceived(self, message): try: # print 'id:', id, 'func:', self.service_callbacks[id] self.service_callbacks[id](values) - except: + except Exception: print("exception on callback ID:", id) traceback.print_exc() raise @@ -136,18 +138,19 @@ def onMessageReceived(self, message): print("Recieved unknown option - it was: ", option) else: print("No OP key!") - except: + except Exception: print("exception in onMessageReceived") print("message", message) traceback.print_exc() raise -class RosbridgeWSConnection: +class RosbridgeWSConnection(): def __init__(self, host, port): - self.ws = websocket.WebSocketApp( - ("ws://%s:%d/" % (host, port)), on_message=self.on_message, on_error=self.on_error, on_close=self.on_close - ) + self.ws = websocket.WebSocketApp(("ws://%s:%d/" % (host, port)), + on_message=self.on_message, + on_error=self.on_error, + on_close=self.on_close) self.ws.on_open = self.on_open self.run_thread = threading.Thread(target=self.run) self.run_thread.start() diff --git a/mir_driver/mir_driver/tf_remove_child_frames.py b/mir_driver/mir_driver/tf_remove_child_frames.py new file mode 100644 index 00000000..064eb346 --- /dev/null +++ b/mir_driver/mir_driver/tf_remove_child_frames.py @@ -0,0 +1,78 @@ +# #!/usr/bin/env python3 +# # -*- coding: utf-8 -*- + +# # Copyright 2016 The Cartographer Authors +# # Copyright 2018 DFKI GmbH +# # +# # Licensed under the Apache License, Version 2.0 (the "License"); +# # you may not use this file except in compliance with the License. +# # You may obtain a copy of the License at +# # +# # http://www.apache.org/licenses/LICENSE-2.0 +# # +# # Unless required by applicable law or agreed to in writing, software +# # distributed under the License is distributed on an "AS IS" BASIS, +# # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# # See the License for the specific language governing permissions and +# # limitations under the License. + +import rclpy +from rclpy.node import Node +from tf2_msgs.msg import TFMessage +from rclpy.qos import QoSProfile, qos_profile_system_default + + +class remove_child_frames_node(Node): + + def __init__(self): + super().__init__('tf_remove_child_frames') + remove_frames = self.declare_parameter('remove_frames', []).value + tf_pub = self.create_publisher( + msg_type=TFMessage, + topic='tf_out', + qos_profile=QoSProfile(depth=1) + ) + + def tf_cb(msg): + msg.transforms = [t for t in msg.transforms + if t.child_frame_id.lstrip('/') not in remove_frames] + if len(msg.transforms) > 0: + tf_pub.publish(msg) + + self.create_subscription( + msg_type=TFMessage, + topic="tf", + callback=tf_cb, + qos_profile=qos_profile_system_default + ) + + tf_static_pub = self.create_publisher( + msg_type=TFMessage, + topic='tf_static_out', + qos_profile=QoSProfile(depth=1) + # latch + ) + + def tf_static_cb(msg): + msg.transforms = [t for t in msg.transforms + if t.child_frame_id.lstrip('/') not in remove_frames] + if len(msg.transforms) > 0: + tf_static_pub.publish(msg) + + self.create_subscription( + msg_type=TFMessage, + topic="tf_static", + callback=tf_static_cb, + qos_profile=qos_profile_system_default + ) + + +def main(args=None): + rclpy.init(args=args) + node = remove_child_frames_node() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/mir_driver/nodes/fake_mir_joint_publisher.py b/mir_driver/nodes/fake_mir_joint_publisher.py deleted file mode 100755 index 65fc81cb..00000000 --- a/mir_driver/nodes/fake_mir_joint_publisher.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 -import rospy -from sensor_msgs.msg import JointState - - -def fake_mir_joint_publisher(): - rospy.init_node('fake_mir_joint_publisher') - prefix = rospy.get_param('~prefix', '') - pub = rospy.Publisher('joint_states', JointState, queue_size=10) - r = rospy.Rate(50.0) - while not rospy.is_shutdown(): - js = JointState() - js.header.stamp = rospy.Time.now() - js.name = [ - prefix + 'left_wheel_joint', - prefix + 'right_wheel_joint', - prefix + 'fl_caster_rotation_joint', - prefix + 'fl_caster_wheel_joint', - prefix + 'fr_caster_rotation_joint', - prefix + 'fr_caster_wheel_joint', - prefix + 'bl_caster_rotation_joint', - prefix + 'bl_caster_wheel_joint', - prefix + 'br_caster_rotation_joint', - prefix + 'br_caster_wheel_joint', - ] - js.position = [0.0 for _ in js.name] - js.velocity = [0.0 for _ in js.name] - js.effort = [0.0 for _ in js.name] - pub.publish(js) - r.sleep() - - -if __name__ == '__main__': - try: - fake_mir_joint_publisher() - except rospy.ROSInterruptException: - pass diff --git a/mir_driver/nodes/mir_bridge.py b/mir_driver/nodes/mir_bridge.py deleted file mode 100755 index 178af583..00000000 --- a/mir_driver/nodes/mir_bridge.py +++ /dev/null @@ -1,495 +0,0 @@ -#!/usr/bin/env python3 -import rospy - -import copy -import sys -from collections.abc import Iterable - -from mir_driver import rosbridge -from rospy_message_converter import message_converter - -from actionlib import SimpleActionClient -import actionlib_msgs.msg -import diagnostic_msgs.msg -import dynamic_reconfigure.msg -import geometry_msgs.msg -import mir_actions.msg -import mir_msgs.msg -import move_base_msgs.msg -import nav_msgs.msg -import rosgraph_msgs.msg -import sdc21x0.msg -import sensor_msgs.msg -import std_msgs.msg -import tf2_msgs.msg -import visualization_msgs.msg - -from collections import OrderedDict - -tf_prefix = '' -static_transforms = OrderedDict() - - -class TopicConfig(object): - def __init__(self, topic, topic_type, latch=False, dict_filter=None): - self.topic = topic - self.topic_type = topic_type - self.latch = latch - self.dict_filter = dict_filter - - -# remap mir_actions/MirMoveBaseAction => move_base_msgs/MoveBaseAction -def _move_base_goal_dict_filter(msg_dict): - filtered_msg_dict = copy.deepcopy(msg_dict) - filtered_msg_dict['goal']['move_task'] = mir_actions.msg.MirMoveBaseGoal.GLOBAL_MOVE - filtered_msg_dict['goal']['goal_dist_threshold'] = 0.25 - filtered_msg_dict['goal']['clear_costmaps'] = True - return filtered_msg_dict - - -def _move_base_feedback_dict_filter(msg_dict): - # filter out slots from the dict that are not in our message definition - # e.g., MiRMoveBaseFeedback has the field "state", which MoveBaseFeedback doesn't - filtered_msg_dict = copy.deepcopy(msg_dict) - filtered_msg_dict['feedback'] = { - key: msg_dict['feedback'][key] for key in move_base_msgs.msg.MoveBaseFeedback.__slots__ - } - return filtered_msg_dict - - -def _move_base_result_dict_filter(msg_dict): - filtered_msg_dict = copy.deepcopy(msg_dict) - filtered_msg_dict['result'] = {key: msg_dict['result'][key] for key in move_base_msgs.msg.MoveBaseResult.__slots__} - return filtered_msg_dict - - -def _cmd_vel_dict_filter(msg_dict): - """ - Converts a geometry_msgs/Twist message dict (as sent from the ROS side) to - a geometry_msgs/TwistStamped message dict (as expected by the MiR on - software version >=2.7). - """ - header = std_msgs.msg.Header(frame_id='', stamp=rospy.Time.now()) - filtered_msg_dict = { - 'header': message_converter.convert_ros_message_to_dictionary(header), - 'twist': copy.deepcopy(msg_dict), - } - return filtered_msg_dict - - -def _tf_dict_filter(msg_dict): - filtered_msg_dict = copy.deepcopy(msg_dict) - for transform in filtered_msg_dict['transforms']: - transform['child_frame_id'] = tf_prefix + '/' + transform['child_frame_id'].strip('/') - return filtered_msg_dict - - -def _tf_static_dict_filter(msg_dict): - """ - The tf_static topic needs special handling. Publishers on tf_static are *latched*, which means that the ROS master - caches the last message that was sent by each publisher on that topic, and will forward it to new subscribers. - However, since the mir_driver node appears to the ROS master as a single node with a single publisher on tf_static, - and there are multiple actual publishers hiding behind it on the MiR side, only one of those messages will be - cached. Therefore, we need to implement the caching ourselves and make sure that we always publish the full set of - transforms as a single message. - """ - global static_transforms - - # prepend tf_prefix - filtered_msg_dict = _tf_dict_filter(msg_dict) - - # The following code was copied + modified from https://github.com/tradr-project/static_transform_mux . - - # Process the incoming transforms, merge them with our cache. - for transform in filtered_msg_dict['transforms']: - key = transform['child_frame_id'] - rospy.loginfo( - "[%s] tf_static: updated transform %s->%s.", - rospy.get_name(), - transform['header']['frame_id'], - transform['child_frame_id'], - ) - static_transforms[key] = transform - - # Return the cached messages. - filtered_msg_dict['transforms'] = static_transforms.values() - rospy.loginfo( - "[%s] tf_static: sent %i transforms: %s", - rospy.get_name(), - len(static_transforms), - str(static_transforms.keys()), - ) - return filtered_msg_dict - - -def _prepend_tf_prefix_dict_filter(msg_dict): - # filtered_msg_dict = copy.deepcopy(msg_dict) - if not isinstance(msg_dict, dict): # can happen during recursion - return - for (key, value) in msg_dict.items(): - if key == 'header': - try: - # prepend frame_id - frame_id = value['frame_id'].strip('/') - if frame_id != 'map': - # prepend tf_prefix, then remove leading '/' (e.g., when tf_prefix is empty) - value['frame_id'] = (tf_prefix + '/' + frame_id).strip('/') - else: - value['frame_id'] = frame_id - - except TypeError: - pass # value is not a dict - except KeyError: - pass # value doesn't have key 'frame_id' - elif isinstance(value, dict): - _prepend_tf_prefix_dict_filter(value) - elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list) - for item in value: - _prepend_tf_prefix_dict_filter(item) - return msg_dict - - -def _remove_tf_prefix_dict_filter(msg_dict): - # filtered_msg_dict = copy.deepcopy(msg_dict) - if not isinstance(msg_dict, dict): # can happen during recursion - return - for (key, value) in msg_dict.items(): - if key == 'header': - try: - # remove frame_id - s = value['frame_id'].strip('/') - if s.find(tf_prefix) == 0: - value['frame_id'] = (s[len(tf_prefix) :]).strip('/') # strip off tf_prefix, then strip leading '/' - except TypeError: - pass # value is not a dict - except KeyError: - pass # value doesn't have key 'frame_id' - elif isinstance(value, dict): - _remove_tf_prefix_dict_filter(value) - elif isinstance(value, Iterable): # an Iterable other than dict (e.g., a list) - for item in value: - _remove_tf_prefix_dict_filter(item) - return msg_dict - - -# topics we want to publish to ROS (and subscribe to from the MiR) -PUB_TOPICS = [ - # TopicConfig('LightCtrl/bms_data', mir_msgs.msg.BMSData), - # TopicConfig('LightCtrl/charging_state', mir_msgs.msg.ChargingState), - TopicConfig('LightCtrl/us_list', sensor_msgs.msg.Range), - # TopicConfig('MC/battery_currents', mir_msgs.msg.BatteryCurrents), - # TopicConfig('MC/battery_voltage', std_msgs.msg.Float64), - TopicConfig('MC/currents', sdc21x0.msg.MotorCurrents), - # TopicConfig('MC/encoders', sdc21x0.msg.StampedEncoders), - TopicConfig('MissionController/CheckArea/visualization_marker', visualization_msgs.msg.Marker), - # TopicConfig('MissionController/goal_position_guid', std_msgs.msg.String), - # TopicConfig('MissionController/prompt_user', mir_msgs.msg.UserPrompt), - TopicConfig('SickPLC/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - TopicConfig('SickPLC/parameter_updates', dynamic_reconfigure.msg.Config), - # TopicConfig('active_mapping_guid', std_msgs.msg.String), - TopicConfig('amcl_pose', geometry_msgs.msg.PoseWithCovarianceStamped), - TopicConfig('b_raw_scan', sensor_msgs.msg.LaserScan), - TopicConfig('b_scan', sensor_msgs.msg.LaserScan), - TopicConfig('camera_floor/background', sensor_msgs.msg.PointCloud2), - TopicConfig('camera_floor/depth/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - TopicConfig('camera_floor/depth/parameter_updates', dynamic_reconfigure.msg.Config), - TopicConfig('camera_floor/depth/points', sensor_msgs.msg.PointCloud2), - TopicConfig('camera_floor/filter/visualization_marker', visualization_msgs.msg.Marker), - TopicConfig('camera_floor/floor', sensor_msgs.msg.PointCloud2), - TopicConfig('camera_floor/obstacles', sensor_msgs.msg.PointCloud2), - TopicConfig('check_area/polygon', geometry_msgs.msg.PolygonStamped), - # TopicConfig('check_pose_area/polygon', geometry_msgs.msg.PolygonStamped), - # TopicConfig('data_events/area_events', mir_data_msgs.msg.AreaEventEvent), - # TopicConfig('data_events/maps', mir_data_msgs.msg.MapEvent), - # TopicConfig('data_events/positions', mir_data_msgs.msg.PositionEvent), - # TopicConfig('data_events/registers', mir_data_msgs.msg.PLCRegisterEvent), - # TopicConfig('data_events/sounds', mir_data_msgs.msg.SoundEvent), - TopicConfig('diagnostics', diagnostic_msgs.msg.DiagnosticArray), - TopicConfig('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray), - TopicConfig('diagnostics_toplevel_state', diagnostic_msgs.msg.DiagnosticStatus), - TopicConfig('f_raw_scan', sensor_msgs.msg.LaserScan), - TopicConfig('f_scan', sensor_msgs.msg.LaserScan), - TopicConfig('imu_data', sensor_msgs.msg.Imu), - TopicConfig('laser_back/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - TopicConfig('laser_back/driver/parameter_updates', dynamic_reconfigure.msg.Config), - TopicConfig('laser_front/driver/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - TopicConfig('laser_front/driver/parameter_updates', dynamic_reconfigure.msg.Config), - # TopicConfig('localization_score', std_msgs.msg.Float64), - TopicConfig('/map', nav_msgs.msg.OccupancyGrid, latch=True), - TopicConfig('/map_metadata', nav_msgs.msg.MapMetaData), - # TopicConfig('marker_tracking_node/feedback', mir_marker_tracking.msg.MarkerTrackingActionFeedback), - # TopicConfig( - # 'marker_tracking_node/laser_line_extract/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription - # ), - # TopicConfig('marker_tracking_node/laser_line_extract/parameter_updates', dynamic_reconfigure.msg.Config), - # TopicConfig('marker_tracking_node/laser_line_extract/visualization_marker', visualization_msgs.msg.Marker), - # TopicConfig('marker_tracking_node/result', mir_marker_tracking.msg.MarkerTrackingActionResult), - # TopicConfig('marker_tracking_node/status', actionlib_msgs.msg.GoalStatusArray), - # TopicConfig('mirEventTrigger/events', mir_msgs.msg.Events), - TopicConfig('mir_amcl/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - TopicConfig('mir_amcl/parameter_updates', dynamic_reconfigure.msg.Config), - TopicConfig('mir_amcl/selected_points', sensor_msgs.msg.PointCloud2), - TopicConfig('mir_log', rosgraph_msgs.msg.Log), - # TopicConfig('mir_sound/sound_event', mir_msgs.msg.SoundEvent), - TopicConfig('mir_status_msg', std_msgs.msg.String), - # TopicConfig('mirspawn/node_events', mirSpawn.msg.LaunchItem), - TopicConfig('mirwebapp/grid_map_metadata', mir_msgs.msg.LocalMapStat), - TopicConfig('mirwebapp/laser_map_metadata', mir_msgs.msg.LocalMapStat), - # TopicConfig('mirwebapp/web_path', mir_msgs.msg.WebPath), - # really mir_actions/MirMoveBaseActionFeedback: - TopicConfig( - 'move_base/feedback', move_base_msgs.msg.MoveBaseActionFeedback, dict_filter=_move_base_feedback_dict_filter - ), - # really mir_actions/MirMoveBaseActionResult: - TopicConfig('move_base/result', move_base_msgs.msg.MoveBaseActionResult, dict_filter=_move_base_result_dict_filter), - TopicConfig('move_base/status', actionlib_msgs.msg.GoalStatusArray), - # TopicConfig('move_base_node/MIRPlannerROS/cost_cloud', sensor_msgs.msg.PointCloud2), - # TopicConfig('move_base_node/MIRPlannerROS/global_plan', nav_msgs.msg.Path), - # TopicConfig('move_base_node/MIRPlannerROS/len_to_goal', std_msgs.msg.Float64), - TopicConfig('move_base_node/MIRPlannerROS/local_plan', nav_msgs.msg.Path), - # TopicConfig('move_base_node/MIRPlannerROS/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - # TopicConfig('move_base_node/MIRPlannerROS/parameter_updates', dynamic_reconfigure.msg.Config), - TopicConfig('move_base_node/MIRPlannerROS/updated_global_plan', mir_msgs.msg.PlanSegments), - # TopicConfig('move_base_node/MIRPlannerROS/visualization_marker', visualization_msgs.msg.MarkerArray), - TopicConfig('move_base_node/SBPLLatticePlanner/plan', nav_msgs.msg.Path), - # TopicConfig( - # 'move_base_node/SBPLLatticePlanner/sbpl_lattice_planner_stats', sbpl_lattice_planner.msg.SBPLLatticePlannerStats - # ), - # TopicConfig('move_base_node/SBPLLatticePlanner/visualization_marker', visualization_msgs.msg.MarkerArray), - TopicConfig('move_base_node/current_goal', geometry_msgs.msg.PoseStamped), - # TopicConfig('move_base_node/global_costmap/inflated_obstacles', nav_msgs.msg.GridCells), - # TopicConfig('move_base_node/global_costmap/obstacles', nav_msgs.msg.GridCells), - # TopicConfig('move_base_node/global_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - # TopicConfig('move_base_node/global_costmap/parameter_updates', dynamic_reconfigure.msg.Config), - # TopicConfig('move_base_node/global_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped), - # TopicConfig('move_base_node/global_costmap/unknown_space', nav_msgs.msg.GridCells), - # TopicConfig('move_base_node/global_plan', nav_msgs.msg.Path), - TopicConfig('move_base_node/local_costmap/inflated_obstacles', nav_msgs.msg.GridCells), - TopicConfig('move_base_node/local_costmap/obstacles', nav_msgs.msg.GridCells), - # TopicConfig('move_base_node/local_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - # TopicConfig('move_base_node/local_costmap/parameter_updates', dynamic_reconfigure.msg.Config), - TopicConfig('move_base_node/local_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped), - # TopicConfig('move_base_node/local_costmap/safety_zone', geometry_msgs.msg.PolygonStamped), - # TopicConfig('move_base_node/local_costmap/unknown_space', nav_msgs.msg.GridCells), - # TopicConfig('move_base_node/mir_escape_recovery/visualization_marker', visualization_msgs.msg.Marker), - # TopicConfig('move_base_node/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - # TopicConfig('move_base_node/parameter_updates', dynamic_reconfigure.msg.Config), - TopicConfig('move_base_node/time_to_coll', std_msgs.msg.Float64), - TopicConfig('move_base_node/traffic_costmap/inflated_obstacles', nav_msgs.msg.GridCells), - TopicConfig('move_base_node/traffic_costmap/obstacles', nav_msgs.msg.GridCells), - TopicConfig('move_base_node/traffic_costmap/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - TopicConfig('move_base_node/traffic_costmap/parameter_updates', dynamic_reconfigure.msg.Config), - TopicConfig('move_base_node/traffic_costmap/robot_footprint', geometry_msgs.msg.PolygonStamped), - TopicConfig('move_base_node/traffic_costmap/unknown_space', nav_msgs.msg.GridCells), - TopicConfig('move_base_node/visualization_marker', visualization_msgs.msg.Marker), - TopicConfig('move_base_simple/visualization_marker', visualization_msgs.msg.Marker), - TopicConfig('odom', nav_msgs.msg.Odometry), - TopicConfig('odom_enc', nav_msgs.msg.Odometry), - # TopicConfig('one_way_map', nav_msgs.msg.OccupancyGrid), - # TopicConfig('param_update', std_msgs.msg.String), - # TopicConfig('particlevizmarker', visualization_msgs.msg.MarkerArray), - # TopicConfig('resource_tracker/needed_resources', mir_msgs.msg.ResourcesState), - TopicConfig('robot_mode', mir_msgs.msg.RobotMode), - TopicConfig('robot_pose', geometry_msgs.msg.Pose), - TopicConfig('robot_state', mir_msgs.msg.RobotState), - # TopicConfig('robot_status', mir_msgs.msg.RobotStatus), - TopicConfig('/rosout', rosgraph_msgs.msg.Log), - TopicConfig('/rosout_agg', rosgraph_msgs.msg.Log), - TopicConfig('scan', sensor_msgs.msg.LaserScan), - # TopicConfig('scan_filter/parameter_descriptions', dynamic_reconfigure.msg.ConfigDescription), - # TopicConfig('scan_filter/parameter_updates', dynamic_reconfigure.msg.Config), - TopicConfig('scan_filter/visualization_marker', visualization_msgs.msg.Marker), - # TopicConfig('session_importer_node/info', mirSessionImporter.msg.SessionImportInfo), - # TopicConfig('set_mc_PID', std_msgs.msg.Float64MultiArray), - TopicConfig('/tf', tf2_msgs.msg.TFMessage, dict_filter=_tf_dict_filter), - TopicConfig('/tf_static', tf2_msgs.msg.TFMessage, dict_filter=_tf_static_dict_filter, latch=True), - # TopicConfig('traffic_map', nav_msgs.msg.OccupancyGrid), - # TopicConfig('wifi_diagnostics', diagnostic_msgs.msg.DiagnosticArray), - # TopicConfig('wifi_diagnostics/cur_ap', mir_wifi_msgs.msg.APInfo), - # TopicConfig('wifi_diagnostics/roam_events', mir_wifi_msgs.msg.WifiRoamEvent), - # TopicConfig('wifi_diagnostics/wifi_ap_interface_stats', mir_wifi_msgs.msg.WifiInterfaceStats), - # TopicConfig('wifi_diagnostics/wifi_ap_rssi', mir_wifi_msgs.msg.APRssiStats), - # TopicConfig('wifi_diagnostics/wifi_ap_time_stats', mir_wifi_msgs.msg.APTimeStats), - # TopicConfig('wifi_watchdog/ping', mir_wifi_msgs.msg.APPingStats), -] - -# topics we want to subscribe to from ROS (and publish to the MiR) -SUB_TOPICS = [ - # really geometry_msgs.msg.TwistStamped: - TopicConfig('cmd_vel', geometry_msgs.msg.Twist, dict_filter=_cmd_vel_dict_filter), - TopicConfig('initialpose', geometry_msgs.msg.PoseWithCovarianceStamped), - TopicConfig('light_cmd', std_msgs.msg.String), - TopicConfig('mir_cmd', std_msgs.msg.String), - TopicConfig('move_base/cancel', actionlib_msgs.msg.GoalID), - # really mir_actions/MirMoveBaseActionGoal: - TopicConfig('move_base/goal', move_base_msgs.msg.MoveBaseActionGoal, dict_filter=_move_base_goal_dict_filter), -] - - -class PublisherWrapper(rospy.SubscribeListener): - def __init__(self, topic_config, robot): - self.topic_config = topic_config - self.robot = robot - self.connected = False - # Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace. - self.pub = rospy.Publisher( - name=topic_config.topic, - data_class=topic_config.topic_type, - subscriber_listener=self, - latch=topic_config.latch, - queue_size=10, - ) - rospy.loginfo( - "[%s] publishing topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type - ) - # latched topics must be subscribed immediately - if topic_config.latch: - self.peer_subscribe("", None, None) - - def peer_subscribe(self, topic_name, topic_publish, peer_publish): - if not self.connected: - self.connected = True - rospy.loginfo("[%s] starting to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic) - absolute_topic = '/' + self.topic_config.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm - self.robot.subscribe(topic=absolute_topic, callback=self.callback) - - def peer_unsubscribe(self, topic_name, num_peers): - pass - # doesn't work: once ubsubscribed, robot doesn't let us subscribe again - # if self.connected and self.pub.get_num_connections() == 0 and not self.topic_config.latch: - # self.connected = False - # rospy.loginfo("[%s] stopping to stream messages on topic '%s'", rospy.get_name(), self.topic_config.topic) - # absolute_topic = '/' + self.topic_config.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm - # self.robot.unsubscribe(topic=absolute_topic) - - def callback(self, msg_dict): - msg_dict = _prepend_tf_prefix_dict_filter(msg_dict) - if self.topic_config.dict_filter is not None: - msg_dict = self.topic_config.dict_filter(msg_dict) - msg = message_converter.convert_dictionary_to_ros_message(self.topic_config.topic_type._type, msg_dict) - self.pub.publish(msg) - - -class SubscriberWrapper(object): - def __init__(self, topic_config, robot): - self.topic_config = topic_config - self.robot = robot - # Use topic_config.topic directly here. If it does not have a leading slash, it will use the private namespace. - self.sub = rospy.Subscriber( - name=topic_config.topic, data_class=topic_config.topic_type, callback=self.callback, queue_size=10 - ) - rospy.loginfo( - "[%s] subscribing to topic '%s' [%s]", rospy.get_name(), topic_config.topic, topic_config.topic_type._type - ) - - def callback(self, msg): - msg_dict = message_converter.convert_ros_message_to_dictionary(msg) - msg_dict = _remove_tf_prefix_dict_filter(msg_dict) - if self.topic_config.dict_filter is not None: - msg_dict = self.topic_config.dict_filter(msg_dict) - absolute_topic = '/' + self.topic_config.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm - self.robot.publish(absolute_topic, msg_dict) - - -class MiR100Bridge(object): - def __init__(self): - try: - hostname = rospy.get_param('~hostname') - except KeyError: - rospy.logfatal('[%s] parameter "hostname" is not set!', rospy.get_name()) - sys.exit(-1) - port = rospy.get_param('~port', 9090) - - global tf_prefix - tf_prefix = rospy.get_param('~tf_prefix', '').strip('/') - - rospy.loginfo('[%s] trying to connect to %s:%i...', rospy.get_name(), hostname, port) - self.robot = rosbridge.RosbridgeSetup(hostname, port) - - r = rospy.Rate(10) - i = 1 - while not self.robot.is_connected(): - if rospy.is_shutdown(): - sys.exit(0) - if self.robot.is_errored(): - rospy.logfatal('[%s] connection error to %s:%i, giving up!', rospy.get_name(), hostname, port) - sys.exit(-1) - if i % 10 == 0: - rospy.logwarn('[%s] still waiting for connection to %s:%i...', rospy.get_name(), hostname, port) - i += 1 - r.sleep() - - rospy.loginfo('[%s] ... connected.', rospy.get_name()) - - topics = self.get_topics() - published_topics = [topic_name for (topic_name, _, has_publishers, _) in topics if has_publishers] - subscribed_topics = [topic_name for (topic_name, _, _, has_subscribers) in topics if has_subscribers] - - for pub_topic in PUB_TOPICS: - PublisherWrapper(pub_topic, self.robot) - absolute_topic = '/' + pub_topic.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm - if absolute_topic not in published_topics: - rospy.logwarn("[%s] topic '%s' is not published by the MiR!", rospy.get_name(), pub_topic.topic) - - for sub_topic in SUB_TOPICS: - SubscriberWrapper(sub_topic, self.robot) - absolute_topic = '/' + sub_topic.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm - if absolute_topic not in subscribed_topics: - rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the MiR!", rospy.get_name(), sub_topic.topic) - - # At least with software version 2.8 there were issues when forwarding a simple goal to the robot - # This workaround converts it into an action. Check https://github.com/dfki-ric/mir_robot/issues/60 for details. - self._move_base_client = SimpleActionClient('move_base', move_base_msgs.msg.MoveBaseAction) - rospy.Subscriber("move_base_simple/goal", geometry_msgs.msg.PoseStamped, self._move_base_simple_goal_callback) - - def get_topics(self): - srv_response = self.robot.callService('/rosapi/topics', msg={}) - topic_names = sorted(srv_response['topics']) - topics = [] - - for topic_name in topic_names: - srv_response = self.robot.callService("/rosapi/topic_type", msg={'topic': topic_name}) - topic_type = srv_response['type'] - - srv_response = self.robot.callService("/rosapi/publishers", msg={'topic': topic_name}) - has_publishers = True if len(srv_response['publishers']) > 0 else False - - srv_response = self.robot.callService("/rosapi/subscribers", msg={'topic': topic_name}) - has_subscribers = True if len(srv_response['subscribers']) > 0 else False - - topics.append([topic_name, topic_type, has_publishers, has_subscribers]) - - print('Publishers:') - for (topic_name, topic_type, has_publishers, has_subscribers) in topics: - if has_publishers: - print((' * %s [%s]' % (topic_name, topic_type))) - - print('\nSubscribers:') - for (topic_name, topic_type, has_publishers, has_subscribers) in topics: - if has_subscribers: - print((' * %s [%s]' % (topic_name, topic_type))) - - return topics - - def _move_base_simple_goal_callback(self, msg): - if not self._move_base_client.wait_for_server(rospy.Duration(2)): - rospy.logwarn("Could not connect to 'move_base' server after two seconds. Dropping goal.") - rospy.logwarn("Did you activate 'planner' in the MIR web interface?") - return - goal = move_base_msgs.msg.MoveBaseGoal() - goal.target_pose.header = copy.deepcopy(msg.header) - goal.target_pose.pose = copy.deepcopy(msg.pose) - self._move_base_client.send_goal(goal) - - -def main(): - rospy.init_node('mir_bridge') - MiR100Bridge() - rospy.spin() - - -if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/mir_driver/nodes/rep117_filter.py b/mir_driver/nodes/rep117_filter.py deleted file mode 100755 index 8434d83f..00000000 --- a/mir_driver/nodes/rep117_filter.py +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -from sensor_msgs.msg import LaserScan - -pub = None - - -def callback(msg): - """ - Convert laser scans to REP 117 standard: - http://www.ros.org/reps/rep-0117.html - """ - ranges_out = [] - for dist in msg.ranges: - if dist < msg.range_min: - # assume "reading too close to measure", - # although it could also be "reading invalid" (nan) - ranges_out.append(float("-inf")) - - elif dist > msg.range_max: - # assume "reading of no return (outside sensor range)", - # although it could also be "reading invalid" (nan) - ranges_out.append(float("inf")) - else: - ranges_out.append(dist) - - msg.ranges = ranges_out - pub.publish(msg) - - -def main(): - global pub - rospy.init_node('rep117_filter') - - pub = rospy.Publisher('scan_filtered', LaserScan, queue_size=10) - rospy.Subscriber('scan', LaserScan, callback) - rospy.spin() - - -if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/mir_driver/nodes/tf_remove_child_frames.py b/mir_driver/nodes/tf_remove_child_frames.py deleted file mode 100755 index 26dcfe15..00000000 --- a/mir_driver/nodes/tf_remove_child_frames.py +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2016 The Cartographer Authors -# Copyright 2018 DFKI GmbH -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import rospy -from tf.msg import tfMessage - - -def main(): - rospy.init_node('tf_remove_child_frames') - remove_frames = rospy.get_param('~remove_frames', []) - - # filter tf_in topic - tf_pub = rospy.Publisher('tf_out', tfMessage, queue_size=1) - - def tf_cb(msg): - msg.transforms = [t for t in msg.transforms if t.child_frame_id.lstrip('/') not in remove_frames] - if len(msg.transforms) > 0: - tf_pub.publish(msg) - - rospy.Subscriber('tf_in', tfMessage, tf_cb) - - # filter tf_static_in topic - tf_static_pub = rospy.Publisher('tf_static_out', tfMessage, queue_size=1, latch=True) - - def tf_static_cb(msg): - msg.transforms = [t for t in msg.transforms if t.child_frame_id.lstrip('/') not in remove_frames] - if len(msg.transforms) > 0: - tf_static_pub.publish(msg) - - rospy.Subscriber('tf_static_in', tfMessage, tf_static_cb) - - rospy.spin() - - -if __name__ == '__main__': - main() diff --git a/mir_driver/package.xml b/mir_driver/package.xml index 7c73dd49..8f66f6b2 100644 --- a/mir_driver/package.xml +++ b/mir_driver/package.xml @@ -2,7 +2,7 @@ mir_driver 1.1.3 - A reverse ROS bridge for the MiR100 robot + A reverse ROS2 bridge for the MiR100 robot Martin Günther Martin Günther @@ -14,27 +14,28 @@ https://github.com/dfki-ric/mir_robot https://github.com/dfki-ric/mir_robot/issues - catkin - - roslaunch - - actionlib_msgs - diagnostic_msgs - dynamic_reconfigure geometry_msgs - mir_actions mir_msgs - move_base_msgs + rclpy_message_converter + ira_laser_tools nav_msgs python3-websocket rosgraph_msgs - rospy - rospy_message_converter + rclpy sensor_msgs std_msgs - tf + tf2_msgs visualization_msgs + twist_stamper mir_description robot_state_publisher + + ament_lint_auto + ament_lint_common + + + ament_python + + diff --git a/mir_driver/resource/mir_driver b/mir_driver/resource/mir_driver new file mode 100644 index 00000000..e69de29b diff --git a/mir_driver/setup.cfg b/mir_driver/setup.cfg new file mode 100644 index 00000000..a1cfb96d --- /dev/null +++ b/mir_driver/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/mir_driver +[install] +install_scripts=$base/lib/mir_driver \ No newline at end of file diff --git a/mir_driver/setup.py b/mir_driver/setup.py index 1ee65923..98a4b840 100644 --- a/mir_driver/setup.py +++ b/mir_driver/setup.py @@ -1,9 +1,31 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +import os +from glob import glob +from setuptools import setup +package_name = 'mir_driver' -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup(packages=['mir_driver'], package_dir={'': 'src'}) - -setup(**setup_args) +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='TODO', + maintainer_email='TODO', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'mir_bridge = mir_driver.mir_bridge:main', + 'fake_mir_joint_publisher = mir_driver.fake_mir_joint_publisher:main', + 'time_synchronizer = mir_driver.time_synchronizer:main', + 'tf_remove_child_frames = mir_driver.tf_remove_child_frames:main' + ], + }, +) diff --git a/mir_dwb_critics/CHANGELOG.rst b/mir_dwb_critics/CHANGELOG.rst deleted file mode 100644 index f97431fc..00000000 --- a/mir_dwb_critics/CHANGELOG.rst +++ /dev/null @@ -1,62 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mir_dwb_critics -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.3 (2021-06-11) ------------------- -* Merge branch 'melodic-2.8' into noetic -* Reformat python code using black -* Contributors: Martin Günther - -1.1.2 (2021-05-12) ------------------- - -1.1.1 (2021-02-11) ------------------- -* Fix bug in path_dist_pruned - With some paths, the previous code crashed with "terminate called after throwing an instance - of 'std::bad_alloc'". -* Contributors: Martin Günther - -1.1.0 (2020-06-30) ------------------- -* Initial release into noetic -* Update scripts to Python3 (Noetic) -* Contributors: Martin Günther - -1.0.6 (2020-06-30) ------------------- -* Add missing matplotlib dependency -* Fix some catkin_lint warnings -* Set cmake_policy CMP0048 to fix warning -* Contributors: Martin Günther - -1.0.5 (2020-05-01) ------------------- -* mir_dwb_critics: Add plot_dwb_scores.py -* mir_dwb_critics: Improve print_dwb_scores output -* added PathDistPrunedCritic for dwb (`#42 `_) - which works exactly like the original PathDistCritic, except that it - searches for a local minimum in the distance from the global path to the robots - current position. It then prunes the global_path from the start up to - this point, therefore approximately cutting of a segment of the path - that the robot already followed. -* Contributors: Martin Günther, Nils Niemann - -1.0.4 (2019-05-06) ------------------- - -1.0.3 (2019-03-04) ------------------- -* PathProgressCritic: Add heading score -* Add package: mir_dwb_critics -* Contributors: Martin Günther - -1.0.2 (2018-07-30) ------------------- - -1.0.1 (2018-07-17) ------------------- - -1.0.0 (2018-07-12) ------------------- diff --git a/mir_dwb_critics/CMakeLists.txt b/mir_dwb_critics/CMakeLists.txt deleted file mode 100644 index 51a6c050..00000000 --- a/mir_dwb_critics/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -cmake_minimum_required(VERSION 3.5.1) -cmake_policy(SET CMP0048 NEW) -project(mir_dwb_critics) - -set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall;-Werror") - -find_package(catkin REQUIRED COMPONENTS - angles - costmap_queue - dwb_critics - dwb_local_planner - geometry_msgs - nav_2d_msgs - nav_2d_utils - nav_core2 - nav_grid_iterators - pluginlib - roscpp - sensor_msgs -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS angles costmap_queue dwb_critics dwb_local_planner geometry_msgs nav_2d_msgs nav_2d_utils nav_core2 nav_grid_iterators pluginlib roscpp sensor_msgs -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} - src/path_angle.cpp - src/path_progress.cpp - src/path_dist_pruned.cpp -) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -install(PROGRAMS - nodes/print_dwb_scores.py - nodes/plot_dwb_scores.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -install(FILES default_critics.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/mir_dwb_critics/default_critics.xml b/mir_dwb_critics/default_critics.xml deleted file mode 100644 index 8c056bf8..00000000 --- a/mir_dwb_critics/default_critics.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - Scores trajectories based on how far along the global path they end up. - - - TODO - - - Scores trajectories based on the distance to the global path, only taking the parts into account that are still ahead of the robot. - - - diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_angle.h b/mir_dwb_critics/include/mir_dwb_critics/path_angle.h deleted file mode 100644 index 0c70097e..00000000 --- a/mir_dwb_critics/include/mir_dwb_critics/path_angle.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, DFKI GmbH - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef MIR_DWB_CRITICS_PATH_ANGLE_H_ -#define MIR_DWB_CRITICS_PATH_ANGLE_H_ - -#include -#include - -namespace mir_dwb_critics -{ -/** - * @class PathAngleCritic - * @brief Scores trajectories based on the difference between the path's current angle and the trajectory's angle - * - * This trajectory critic helps to ensure that the robot is roughly aligned with the path (i.e., - * if the path specifies a forward motion, the robot should point forward along the path and vice versa). - * This critic is not a replacement for the PathAlignCritic: The PathAlignCritic tries to point the robot - * towards a point on the path that is in front of the trajectory's end point, so it helps guiding the - * robot back towards the path. The PathAngleCritic on the other hand uses the path point that is closest - * to the robot's current position (not the trajectory end point, or even a point in front of that) as a - * reference, so it always lags behind. Also, it tries to keep the robot parallel to the path, not towards - * it. For these reasons, the error is squared, so that the PathAngleCritic really only affects the final - * score if the error is large. The PathAlignCritic doesn't take the path orientation into account though, - * so that's why the PathAngleCritic is a useful addition. - */ -class PathAngleCritic: public dwb_local_planner::TrajectoryCritic -{ -public: - virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel, - const geometry_msgs::Pose2D& goal, - const nav_2d_msgs::Path2D& global_plan) override; - - virtual double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override; - -protected: - double desired_angle_; -}; - -} // namespace mir_dwb_critics -#endif // MIR_DWB_CRITICS_PATH_ANGLE_H_ diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_dist_pruned.h b/mir_dwb_critics/include/mir_dwb_critics/path_dist_pruned.h deleted file mode 100644 index 81e57388..00000000 --- a/mir_dwb_critics/include/mir_dwb_critics/path_dist_pruned.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, DFKI GmbH - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef MIR_DWB_CRITICS_PATH_DISTANCE_PRUNED_H_ -#define MIR_DWB_CRITICS_PATH_DISTANCE_PRUNED_H_ - -#include -#include - -namespace mir_dwb_critics -{ -/** - * @class PathDistPrunedCritic - * @brief Scores trajectories based on how far from the global path they end up, - * where the global path is pruned to only include waypoints ahead of the - * robot. - */ -class PathDistPrunedCritic : public dwb_critics::PathDistCritic { -public: - virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel, - const geometry_msgs::Pose2D& goal, - const nav_2d_msgs::Path2D& global_plan) override; -}; - -} // namespace mir_dwb_critics -#endif // MIR_DWB_CRITICS_DISTANCE_PRUNED_H_ diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h deleted file mode 100644 index d0fa9e5e..00000000 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, DFKI GmbH - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef MIR_DWB_CRITICS_PATH_PROGRESS_H_ -#define MIR_DWB_CRITICS_PATH_PROGRESS_H_ - -#include -#include - -namespace mir_dwb_critics { -/** - * @class PathProgressCritic - * @brief Calculates an intermediate goal along the global path and scores trajectories on the distance to that goal. - * - * This trajectory critic helps ensure progress along the global path. It - * calculates an intermediate goal that is as far as possible along the global - * path as long as the path continues to move in one direction (+/- - * angle_threshold). - */ -class PathProgressCritic : public dwb_critics::MapGridCritic { - public: - bool prepare(const geometry_msgs::Pose2D &pose, - const nav_2d_msgs::Twist2D &vel, - const geometry_msgs::Pose2D &goal, - const nav_2d_msgs::Path2D &global_plan) override; - - void onInit() override; - void reset() override; - double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override; - - protected: - bool getGoalPose(const geometry_msgs::Pose2D &robot_pose, - const nav_2d_msgs::Path2D &global_plan, - unsigned int &x, - unsigned int &y, - double &desired_angle); - - unsigned int getGoalIndex(const std::vector &plan, - unsigned int start_index, - unsigned int last_valid_index) const; - - double xy_local_goal_tolerance_; - double angle_threshold_; - double heading_scale_; - - std::vector reached_intermediate_goals_; - double desired_angle_; -}; - -} // namespace mir_dwb_critics -#endif // MIR_DWB_CRITICS_PATH_PROGRESS_H_ diff --git a/mir_dwb_critics/nodes/plot_dwb_scores.py b/mir_dwb_critics/nodes/plot_dwb_scores.py deleted file mode 100755 index 9c911042..00000000 --- a/mir_dwb_critics/nodes/plot_dwb_scores.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python3 -import os -import tkinter -import matplotlib.pyplot as plt -import numpy as np - -import rospy -from dwb_msgs.msg import LocalPlanEvaluation - -fig = None -rects = None -max_score = 0.0 - - -def eval_cb(msg): - global fig, rects, max_score - labels = [s.name for s in msg.twists[msg.best_index].scores] - scaled_scores = [s.scale * s.raw_score for s in msg.twists[msg.best_index].scores] - - # reverse labels + scaled_scores to show the critics in the correct order top to bottom - labels.reverse() - scaled_scores.reverse() - - if not fig: - # init - y = np.arange(len(labels)) # the label locations - height = 0.35 # the height of the bars - - fig, ax = plt.subplots() - rects = ax.barh(y - height / 2, scaled_scores, height, label='scaled score') - - ax.set_ylabel('DWB Critics') - ax.set_title('Scaled scores') - ax.set_yticks(y) - ax.set_yticklabels(labels) - - fig.tight_layout() - fig.canvas.set_window_title('DWB Scores') - - # update axis limit - if max_score < max(scaled_scores): - max_score = max(scaled_scores) - plt.xlim(0.0, max_score) - - for rect, h in zip(rects, scaled_scores): - rect.set_width(h) - try: - fig.canvas.draw() - fig.canvas.flush_events() - except tkinter.TclError: - rospy.loginfo('Window was closed, exiting.') - os._exit(0) - - -def main(): - rospy.init_node('plot_dwb_scores') - rospy.Subscriber('move_base_node/DWBLocalPlanner/evaluation', LocalPlanEvaluation, eval_cb) - rospy.loginfo('plot_dwb_scores ready.') - plt.ion() - rospy.spin() - - -if __name__ == '__main__': - main() diff --git a/mir_dwb_critics/nodes/print_dwb_scores.py b/mir_dwb_critics/nodes/print_dwb_scores.py deleted file mode 100755 index c61a067a..00000000 --- a/mir_dwb_critics/nodes/print_dwb_scores.py +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/env python3 -import rospy - -from dwb_msgs.msg import LocalPlanEvaluation - - -def eval_cb(msg): - print('\n\n=========================================================\n\n') - for heading, i in zip(['best trajectory', 'worst trajectory'], [msg.best_index, msg.worst_index]): - print('### {}\n'.format(heading)) - print('Name | Raw | Scale | Scaled Score') - print('---------------------|-----------|---------|-------------') - for s in msg.twists[i].scores: - print('{:20} | {:9.4f} | {:7.4f} | {:12.4f}'.format(s.name, s.raw_score, s.scale, s.raw_score * s.scale)) - print('---------------------------------------- total: {:9.4f}'.format(msg.twists[i].total)) - print() - - -def main(): - rospy.init_node('print_dwb_scores', anonymous=True) - rospy.Subscriber('move_base_node/DWBLocalPlanner/evaluation', LocalPlanEvaluation, eval_cb) - rospy.loginfo('print_dwb_scores ready.') - rospy.spin() - - -if __name__ == '__main__': - main() diff --git a/mir_dwb_critics/package.xml b/mir_dwb_critics/package.xml deleted file mode 100644 index 7cccdc89..00000000 --- a/mir_dwb_critics/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - mir_dwb_critics - 1.1.3 - - Trajectory critics for the dwb_local_planner that work well together with the SBPL global planner on the MiR robot - - - Martin Günther - Martin Günther - - BSD - - https://github.com/dfki-ric/mir_dwb_critics - https://github.com/dfki-ric/mir_dwb_critics - https://github.com/dfki-ric/mir_dwb_critics/issues - - catkin - - angles - costmap_queue - dwb_critics - dwb_local_planner - geometry_msgs - nav_2d_msgs - nav_2d_utils - nav_core2 - nav_grid_iterators - pluginlib - roscpp - sensor_msgs - - python3-matplotlib - - - - - diff --git a/mir_dwb_critics/src/path_angle.cpp b/mir_dwb_critics/src/path_angle.cpp deleted file mode 100644 index f4d9e265..00000000 --- a/mir_dwb_critics/src/path_angle.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, DFKI GmbH - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include - -namespace mir_dwb_critics { - -bool PathAngleCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel, - const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) { - const nav_core2::Costmap &costmap = *costmap_; - const nav_grid::NavGridInfo &info = costmap.getInfo(); - nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution); - - if (global_plan.poses.empty()) { - ROS_ERROR_NAMED("PathAngleCritic", "The global plan was empty."); - return false; - } - - // find the angle of the plan at the pose on the plan closest to the robot - double distance_min = std::numeric_limits::infinity(); - bool started_path = false; - for (unsigned int i = 0; i < adjusted_global_plan.poses.size(); i++) { - double g_x = adjusted_global_plan.poses[i].x; - double g_y = adjusted_global_plan.poses[i].y; - unsigned int map_x, map_y; - if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION) { - double distance = nav_2d_utils::poseDistance(adjusted_global_plan.poses[i], pose); - if (distance_min > distance) { - // still getting closer - desired_angle_ = adjusted_global_plan.poses[i].theta; - distance_min = distance; - started_path = true; - } else { - // plan is going away from the robot again - break; - } - } else if (started_path) { - // Off the costmap after being on the costmap. - break; - } - // else, we have not yet found a point on the costmap, so we just continue - } - - if (!started_path) { - ROS_ERROR_NAMED("PathAngleCritic", "None of the points of the global plan were in the local costmap."); - return false; - } - return true; -} - -double PathAngleCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj) { - double diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI)); - return diff * diff; -} - -} // namespace mir_dwb_critics - -PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathAngleCritic, dwb_local_planner::TrajectoryCritic) diff --git a/mir_dwb_critics/src/path_dist_pruned.cpp b/mir_dwb_critics/src/path_dist_pruned.cpp deleted file mode 100644 index d0ccc165..00000000 --- a/mir_dwb_critics/src/path_dist_pruned.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, DFKI GmbH - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#include -#include - -#include "mir_dwb_critics/path_dist_pruned.h" - -namespace mir_dwb_critics { - -bool PathDistPrunedCritic::prepare( - const geometry_msgs::Pose2D &pose, - const nav_2d_msgs::Twist2D &vel, - const geometry_msgs::Pose2D &goal, - const nav_2d_msgs::Path2D &global_plan) { - - const nav_core2::Costmap &costmap = *costmap_; - const nav_grid::NavGridInfo &info = costmap.getInfo(); - - auto plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; - - - // --- stolen from PathProgressCritic::getGoalPose --- - // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map - unsigned int start_index = 0; - double distance_to_start = std::numeric_limits::infinity(); - bool started_path = false; - for (unsigned int i = 0; i < plan.size(); i++) { - double g_x = plan[i].x; - double g_y = plan[i].y; - unsigned int map_x, map_y; - if (worldToGridBounded(info, g_x, g_y, map_x, map_y) - && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) { - // Still on the costmap. Continue. - double distance = nav_2d_utils::poseDistance(plan[i], pose); - if (distance_to_start > distance) { - start_index = i; - distance_to_start = distance; - started_path = true; - } else { - // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's - // even closer to the robot, but then we would skip over parts of the plan. - break; - } - } else if (started_path) { - // Off the costmap after being on the costmap. - break; - } - // else, we have not yet found a point on the costmap, so we just continue - } - - if (!started_path) { - ROS_ERROR_NAMED("PathDistPrunedCritic", "None of the points of the global plan were in the local costmap."); - return false; - } - // --------------------------------------------------- - - // remove the first part of the path, everything before start_index, to - // disregard that part in the PathDistCritic implementation. - nav_2d_msgs::Path2D global_plan_pruned; - global_plan_pruned.header = global_plan.header; - global_plan_pruned.poses = std::vector( - plan.begin() + start_index, - plan.end()); - - return dwb_critics::PathDistCritic::prepare(pose, vel, goal, global_plan_pruned); -} - -} // namespace mir_dwb_critics - -PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathDistPrunedCritic, dwb_local_planner::TrajectoryCritic) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp deleted file mode 100644 index ff09fe28..00000000 --- a/mir_dwb_critics/src/path_progress.cpp +++ /dev/null @@ -1,221 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, DFKI GmbH - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#include -#include -#include -#include -#include - -namespace mir_dwb_critics { -bool PathProgressCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel, - const geometry_msgs::Pose2D &goal, - const nav_2d_msgs::Path2D &global_plan) { - dwb_critics::MapGridCritic::reset(); - - unsigned int local_goal_x, local_goal_y; - if (!getGoalPose(pose, global_plan, local_goal_x, local_goal_y, desired_angle_)) { - return false; - } - - // Enqueue just the last pose - cell_values_.setValue(local_goal_x, local_goal_y, 0.0); - queue_->enqueueCell(local_goal_x, local_goal_y); - - propogateManhattanDistances(); - - return true; -} - -void PathProgressCritic::onInit() { - dwb_critics::MapGridCritic::onInit(); - critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20); - critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4); - critic_nh_.param("heading_scale", heading_scale_, 1.0); - - // divide heading scale by position scale because the sum will be multiplied by scale again - heading_scale_ /= getScale(); -} - -void PathProgressCritic::reset() { - reached_intermediate_goals_.clear(); -} - -double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj) { - double position_score = MapGridCritic::scoreTrajectory(traj); - double heading_diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI)); - double heading_score = heading_diff * heading_diff; - - return position_score + heading_scale_ * heading_score; -} - -bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan, - unsigned int &x, unsigned int &y, double &desired_angle) { - const nav_core2::Costmap &costmap = *costmap_; - const nav_grid::NavGridInfo &info = costmap.getInfo(); - - if (global_plan.poses.empty()) { - ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty."); - return false; - } - - std::vector plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; - - // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map - unsigned int start_index = 0; - double distance_to_start = std::numeric_limits::infinity(); - bool started_path = false; - for (unsigned int i = 0; i < plan.size(); i++) { - double g_x = plan[i].x; - double g_y = plan[i].y; - unsigned int map_x, map_y; - if (worldToGridBounded(info, g_x, g_y, map_x, map_y) - && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) { - // Still on the costmap. Continue. - double distance = nav_2d_utils::poseDistance(plan[i], robot_pose); - if (distance_to_start > distance) { - start_index = i; - distance_to_start = distance; - started_path = true; - } else { - // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's - // even closer to the robot, but then we would skip over parts of the plan. - break; - } - } else if (started_path) { - // Off the costmap after being on the costmap. - break; - } - // else, we have not yet found a point on the costmap, so we just continue - } - - if (!started_path) { - ROS_ERROR_NAMED("PathProgressCritic", "None of the points of the global plan were in the local costmap."); - return false; - } - - // find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map - unsigned int last_valid_index = start_index; - for (unsigned int i = start_index + 1; i < plan.size(); i++) { - double g_x = plan[i].x; - double g_y = plan[i].y; - unsigned int map_x, map_y; - if (worldToGridBounded(info, g_x, g_y, map_x, map_y) - && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) { - // Still on the costmap. Continue. - last_valid_index = i; - } else { - // Off the costmap after being on the costmap. - break; - } - } - - // find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point, - // i.e. is within angle_threshold_ of the starting direction. - unsigned int goal_index = start_index; - - while (goal_index < last_valid_index) { - goal_index = getGoalIndex(plan, start_index, last_valid_index); - - // check if goal already reached - bool goal_already_reached = false; - for (auto &reached_intermediate_goal : reached_intermediate_goals_) { - double distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]); - if (distance < xy_local_goal_tolerance_) { - goal_already_reached = true; - // choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again - // (start_index is now > goal_index) - for (start_index = goal_index; start_index <= last_valid_index; ++start_index) { - distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]); - if (distance >= xy_local_goal_tolerance_) { - break; - } - } - break; - } - } - if (!goal_already_reached) { - // new goal not in reached_intermediate_goals_ - double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose); - if (distance < xy_local_goal_tolerance_) { - // the robot has currently reached the goal - reached_intermediate_goals_.push_back(plan[goal_index]); - ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size()); - } else { - // we've found a new goal! - break; - } - } - } - if (start_index > goal_index) - start_index = goal_index; - ROS_ASSERT(goal_index <= last_valid_index); - - // save goal in x, y - worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y); - desired_angle = plan[start_index].theta; - return true; -} - -unsigned int PathProgressCritic::getGoalIndex(const std::vector &plan, - unsigned int start_index, - unsigned int last_valid_index) const { - if (start_index >= last_valid_index) - return last_valid_index; - - unsigned int goal_index = start_index; - const double start_direction_x = plan[start_index + 1].x - plan[start_index].x; - const double start_direction_y = plan[start_index + 1].y - plan[start_index].y; - if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) { // make sure that atan2 is defined - double start_angle = atan2(start_direction_y, start_direction_x); - - for (unsigned int i = start_index + 1; i <= last_valid_index; ++i) { - const double current_direction_x = plan[i].x - plan[i - 1].x; - const double current_direction_y = plan[i].y - plan[i - 1].y; - if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) { - double current_angle = atan2(current_direction_y, current_direction_x); - - // goal pose is found if plan doesn't point far enough away from the starting point - if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > angle_threshold_) - break; - - goal_index = i; - } - } - } - return goal_index; -} - -} // namespace mir_dwb_critics - -PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathProgressCritic, dwb_local_planner::TrajectoryCritic) diff --git a/mir_gazebo/CMakeLists.txt b/mir_gazebo/CMakeLists.txt index fcc068e3..cbb284d0 100644 --- a/mir_gazebo/CMakeLists.txt +++ b/mir_gazebo/CMakeLists.txt @@ -1,31 +1,36 @@ cmake_minimum_required(VERSION 3.5.1) -cmake_policy(SET CMP0048 NEW) project(mir_gazebo) -find_package(catkin REQUIRED COMPONENTS - roslaunch -) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -################################### -## catkin specific configuration ## -################################### -catkin_package() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -############# -## Install ## -############# +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(mir_description REQUIRED) +find_package(gazebo REQUIRED) +find_package(gazebo_ros REQUIRED) +find_package(gazebo_ros_pkgs REQUIRED) +find_package(ira_laser_tools REQUIRED) -# Mark other files for installation (e.g. launch and bag files, etc.) install(DIRECTORY - config - launch - maps - sdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + launch worlds maps rviz + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() -roslaunch_add_file_check(launch) +ament_package() diff --git a/mir_gazebo/launch/fake_localization.launch b/mir_gazebo/launch/fake_localization.launch deleted file mode 100644 index 3bbbfe15..00000000 --- a/mir_gazebo/launch/fake_localization.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/mir_gazebo/launch/include/mir_gazebo_common.py b/mir_gazebo/launch/include/mir_gazebo_common.py new file mode 100644 index 00000000..ec20fb8b --- /dev/null +++ b/mir_gazebo/launch/include/mir_gazebo_common.py @@ -0,0 +1,33 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + namespace = LaunchConfiguration('namespace', default='') + + return LaunchDescription([ + + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation (Gazebo) clock if true'), + + Node( + package='ira_laser_tools', + name='mir_laser_scan_merger', + executable='laserscan_multi_merger', + parameters=[{'laserscan_topics': "b_scan f_scan", + 'destination_frame': "virtual_laser_link", + 'scan_destination_topic': "scan", + 'cloud_destination_topic': "scan_cloud", + 'min_height': -0.25, + 'max_completion_time': 0.05, + 'max_merge_time_diff': 0.005, + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'best_effort': False}], + namespace=namespace, # adds namespace to topic names and frames + output='screen') + ]) diff --git a/mir_gazebo/launch/includes/ekf.launch.xml b/mir_gazebo/launch/includes/ekf.launch.xml deleted file mode 100644 index bcf3362d..00000000 --- a/mir_gazebo/launch/includes/ekf.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/mir_gazebo/launch/includes/spawn_maze.launch.xml b/mir_gazebo/launch/includes/spawn_maze.launch.xml deleted file mode 100644 index 604f501c..00000000 --- a/mir_gazebo/launch/includes/spawn_maze.launch.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/mir_gazebo/launch/mir_empty_world.launch b/mir_gazebo/launch/mir_empty_world.launch deleted file mode 100644 index b3576e06..00000000 --- a/mir_gazebo/launch/mir_empty_world.launch +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mir_gazebo/launch/mir_gazebo_common.launch b/mir_gazebo/launch/mir_gazebo_common.launch deleted file mode 100644 index 656c22b4..00000000 --- a/mir_gazebo/launch/mir_gazebo_common.launch +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [mir/joint_states] - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mir_gazebo/launch/mir_gazebo_launch.py b/mir_gazebo/launch/mir_gazebo_launch.py new file mode 100644 index 00000000..8e047d5b --- /dev/null +++ b/mir_gazebo/launch/mir_gazebo_launch.py @@ -0,0 +1,157 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, \ + SetLaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + mir_description_dir = get_package_share_directory('mir_description') + mir_gazebo_dir = get_package_share_directory('mir_gazebo') + gazebo_ros_dir = get_package_share_directory('gazebo_ros') + + rviz_config_file = LaunchConfiguration('rviz_config_file') + + ld = LaunchDescription() + + declare_namespace_arg = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Namespace to push all topics into.') + + declare_robot_x_arg = DeclareLaunchArgument( + 'robot_x', + default_value='0.0', + description='Spawning position of robot (x)') + + declare_robot_y_arg = DeclareLaunchArgument( + 'robot_y', + default_value='0.0', + description='Spawning position of robot (y)') + + declare_robot_yaw_arg = DeclareLaunchArgument( + 'robot_yaw', + default_value='0.0', + description='Spawning position of robot (yaw)') + + declare_sim_time_arg = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation (Gazebo) clock if true') + + declare_world_arg = DeclareLaunchArgument( + 'world', + default_value='empty', + description='Choose simulation world. Available worlds: empty, maze') + + declare_verbose_arg = DeclareLaunchArgument( + 'verbose', + default_value='false', + description='Set to true to enable verbose mode for Gazebo.') + + declare_teleop_arg = DeclareLaunchArgument( + 'teleop_enabled', + default_value='true', + description='Set to true to enable teleop to manually move MiR around.') + + declare_rviz_arg = DeclareLaunchArgument( + 'rviz_enabled', + default_value='true', + description='Set to true to launch rviz.') + + declare_rviz_config_arg = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join( + mir_description_dir, 'rviz', 'mir_visu_full.rviz'), + description='Define rviz config file to be used.') + + declare_gui_arg = DeclareLaunchArgument( + 'gui', + default_value='true', + description='Set to "false" to run headless.') + + launch_gazebo_world = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(gazebo_ros_dir, 'launch', 'gazebo.launch.py')), + launch_arguments={ + 'verbose': LaunchConfiguration('verbose'), + 'gui': LaunchConfiguration('gui'), + 'world': [mir_gazebo_dir, '/worlds/', LaunchConfiguration('world'), '.world'] + }.items() + ) + + launch_mir_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_description_dir, 'launch', 'mir_launch.py') + ) + ) + + launch_mir_gazebo_common = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_gazebo_dir, 'launch', + 'include', 'mir_gazebo_common.py') + ) + ) + + def process_namespace(context): + robot_name = "mir_robot" + try: + namespace = context.launch_configurations['namespace'] + robot_name = namespace + '/' + robot_name + except KeyError: + pass + return [SetLaunchConfiguration('robot_name', robot_name)] + + spawn_robot = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-entity', LaunchConfiguration('robot_name'), + '-topic', 'robot_description', + '-b'], # bond node to gazebo model, + namespace=LaunchConfiguration('namespace'), + output='screen') + + launch_rviz = Node( + condition=IfCondition(LaunchConfiguration('rviz_enabled')), + package='rviz2', + executable='rviz2', + output={'both': 'log'}, + arguments=['-d', rviz_config_file], + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}] + ) + + launch_teleop = Node( + condition=IfCondition(LaunchConfiguration("teleop_enabled")), + package='teleop_twist_keyboard', + executable='teleop_twist_keyboard', + namespace=LaunchConfiguration('namespace'), + output='screen', + prefix='xterm -e') + + ld.add_action(OpaqueFunction(function=process_namespace)) + ld.add_action(declare_namespace_arg) + ld.add_action(declare_robot_x_arg) + ld.add_action(declare_robot_y_arg) + ld.add_action(declare_robot_yaw_arg) + ld.add_action(declare_sim_time_arg) + ld.add_action(declare_world_arg) + ld.add_action(declare_verbose_arg) + ld.add_action(declare_teleop_arg) + ld.add_action(declare_rviz_arg) + ld.add_action(declare_rviz_config_arg) + ld.add_action(declare_gui_arg) + + ld.add_action(launch_gazebo_world) + ld.add_action(launch_mir_description) + ld.add_action(launch_mir_gazebo_common) + ld.add_action(spawn_robot) + ld.add_action(launch_rviz) + ld.add_action(launch_teleop) + + return ld diff --git a/mir_gazebo/launch/mir_maze_world.launch b/mir_gazebo/launch/mir_maze_world.launch deleted file mode 100644 index 1f3f2a09..00000000 --- a/mir_gazebo/launch/mir_maze_world.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/mir_gazebo/package.xml b/mir_gazebo/package.xml index 71a35c2d..7bf7c56e 100644 --- a/mir_gazebo/package.xml +++ b/mir_gazebo/package.xml @@ -1,5 +1,5 @@ - + mir_gazebo 1.1.3 Simulation specific launch and configuration files for the MiR100 robot. @@ -13,19 +13,24 @@ https://github.com/dfki-ric/mir_robot https://github.com/dfki-ric/mir_robot/issues - catkin + ament_cmake - roslaunch + gazebo + gazebo_ros + rviz2 + ira_laser_tools + teleop_twist_keyboard + xterm + xacro + mir_description + gazebo_ros_pkgs + robot_localization - controller_manager - fake_localization - gazebo_ros - joint_state_publisher - mir_description - mir_driver - robot_localization - robot_state_publisher - rostopic - rqt_robot_steering - topic_tools + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mir_gazebo/rviz/mir_visualization.rviz b/mir_gazebo/rviz/mir_visualization.rviz new file mode 100644 index 00000000..4db4158b --- /dev/null +++ b/mir_gazebo/rviz/mir_visualization.rviz @@ -0,0 +1,353 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /RobotModel1/Description Topic1 + - /LaserScan1 + Splitter Ratio: 0.5 + Tree Height: 784 + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + surface: + Alpha: 1 + Show Axes: false + Show Trail: false + us_1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + us_2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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/LaserScan + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.03999999910593033 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + back_laser_link: + Value: true + base_footprint: + Value: true + base_link: + Value: true + bl_caster_rotation_link: + Value: true + bl_caster_wheel_link: + Value: true + br_caster_rotation_link: + Value: true + br_caster_wheel_link: + Value: true + fl_caster_rotation_link: + Value: true + fl_caster_wheel_link: + Value: true + fr_caster_rotation_link: + Value: true + fr_caster_wheel_link: + Value: true + front_laser_link: + Value: true + imu_frame: + Value: true + imu_link: + Value: true + left_wheel_link: + Value: true + odom: + Value: true + right_wheel_link: + Value: true + surface: + Value: true + us_1_frame: + Value: true + us_2_frame: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_footprint: + base_link: + back_laser_link: + {} + bl_caster_rotation_link: + bl_caster_wheel_link: + {} + br_caster_rotation_link: + br_caster_wheel_link: + {} + fl_caster_rotation_link: + fl_caster_wheel_link: + {} + fr_caster_rotation_link: + fr_caster_wheel_link: + {} + front_laser_link: + {} + imu_link: + imu_frame: + {} + left_wheel_link: + {} + right_wheel_link: + {} + surface: + {} + us_1_frame: + {} + us_2_frame: + {} + odom: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + 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 + 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: /move_base_simple/goal + - 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: 33.73630905151367 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -2.321113348007202 + Y: -0.5046675205230713 + Z: 0.6754636764526367 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5397963523864746 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.004999637603759766 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004b30000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/mir_gazebo/worlds/empty.world b/mir_gazebo/worlds/empty.world new file mode 100644 index 00000000..53216724 --- /dev/null +++ b/mir_gazebo/worlds/empty.world @@ -0,0 +1,13 @@ + + + + + + model://sun + + + + model://ground_plane + + + diff --git a/mir_gazebo/worlds/include/ground_plane/model-1_2.sdf b/mir_gazebo/worlds/include/ground_plane/model-1_2.sdf new file mode 100644 index 00000000..8950632f --- /dev/null +++ b/mir_gazebo/worlds/include/ground_plane/model-1_2.sdf @@ -0,0 +1,39 @@ + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 100 100 + + + + + + + + + diff --git a/mir_gazebo/worlds/include/ground_plane/model-1_3.sdf b/mir_gazebo/worlds/include/ground_plane/model-1_3.sdf new file mode 100644 index 00000000..a3fb7baf --- /dev/null +++ b/mir_gazebo/worlds/include/ground_plane/model-1_3.sdf @@ -0,0 +1,39 @@ + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 100 100 + + + + + + + + + diff --git a/mir_gazebo/worlds/include/ground_plane/model-1_4.sdf b/mir_gazebo/worlds/include/ground_plane/model-1_4.sdf new file mode 100644 index 00000000..7fc9b5ea --- /dev/null +++ b/mir_gazebo/worlds/include/ground_plane/model-1_4.sdf @@ -0,0 +1,39 @@ + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 100 100 + + + + + + + + + diff --git a/mir_gazebo/worlds/include/ground_plane/model.config b/mir_gazebo/worlds/include/ground_plane/model.config new file mode 100644 index 00000000..f586097c --- /dev/null +++ b/mir_gazebo/worlds/include/ground_plane/model.config @@ -0,0 +1,19 @@ + + + + Ground Plane + 1.0 + model-1_2.sdf + model-1_3.sdf + model-1_4.sdf + model.sdf + + + Nate Koenig + nate@osrfoundation.org + + + + A simple ground plane. + + diff --git a/mir_gazebo/worlds/include/ground_plane/model.sdf b/mir_gazebo/worlds/include/ground_plane/model.sdf new file mode 100644 index 00000000..00fe113e --- /dev/null +++ b/mir_gazebo/worlds/include/ground_plane/model.sdf @@ -0,0 +1,39 @@ + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 100 100 + + + + + + + + + diff --git a/mir_gazebo/sdf/maze/model.config b/mir_gazebo/worlds/include/maze/model.config similarity index 100% rename from mir_gazebo/sdf/maze/model.config rename to mir_gazebo/worlds/include/maze/model.config diff --git a/mir_gazebo/sdf/maze/model.sdf b/mir_gazebo/worlds/include/maze/model.sdf similarity index 100% rename from mir_gazebo/sdf/maze/model.sdf rename to mir_gazebo/worlds/include/maze/model.sdf diff --git a/mir_gazebo/worlds/include/sun/model-1_2.sdf b/mir_gazebo/worlds/include/sun/model-1_2.sdf new file mode 100644 index 00000000..af51c66e --- /dev/null +++ b/mir_gazebo/worlds/include/sun/model-1_2.sdf @@ -0,0 +1,19 @@ + + + + true + + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.1 0.1 0.1 1 + + + 1000 + 0.9 + 0.01 + 0.001 + + + -0.5 0.5 -1.0 + + diff --git a/mir_gazebo/worlds/include/sun/model-1_3.sdf b/mir_gazebo/worlds/include/sun/model-1_3.sdf new file mode 100644 index 00000000..2f50ffe7 --- /dev/null +++ b/mir_gazebo/worlds/include/sun/model-1_3.sdf @@ -0,0 +1,19 @@ + + + + true + + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.1 0.1 0.1 1 + + + 1000 + 0.9 + 0.01 + 0.001 + + + -0.5 0.5 -1.0 + + diff --git a/mir_gazebo/worlds/include/sun/model-1_4.sdf b/mir_gazebo/worlds/include/sun/model-1_4.sdf new file mode 100644 index 00000000..94301a15 --- /dev/null +++ b/mir_gazebo/worlds/include/sun/model-1_4.sdf @@ -0,0 +1,20 @@ + + + + + true + + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + + 1000 + 0.9 + 0.01 + 0.001 + + + -0.5 0.1 -0.9 + + diff --git a/mir_gazebo/worlds/include/sun/model.config b/mir_gazebo/worlds/include/sun/model.config new file mode 100644 index 00000000..aa569f9f --- /dev/null +++ b/mir_gazebo/worlds/include/sun/model.config @@ -0,0 +1,19 @@ + + + + Sun + 1.0 + model-1_2.sdf + model-1_3.sdf + model-1_4.sdf + model.sdf + + + Nate Koenig + nate@osrfoundation.org + + + + A directional light for the sun. + + diff --git a/mir_gazebo/worlds/include/sun/model.sdf b/mir_gazebo/worlds/include/sun/model.sdf new file mode 100644 index 00000000..ca996256 --- /dev/null +++ b/mir_gazebo/worlds/include/sun/model.sdf @@ -0,0 +1,17 @@ + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + diff --git a/mir_gazebo/worlds/maze.world b/mir_gazebo/worlds/maze.world new file mode 100644 index 00000000..9109690e --- /dev/null +++ b/mir_gazebo/worlds/maze.world @@ -0,0 +1,15 @@ + + + + + model://sun + + + + model://ground_plane + + + model://maze + + + diff --git a/mir_msgs/CHANGELOG.rst b/mir_msgs/CHANGELOG.rst index e386ea6d..0684a5b5 100644 --- a/mir_msgs/CHANGELOG.rst +++ b/mir_msgs/CHANGELOG.rst @@ -2,18 +2,6 @@ Changelog for package mir_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -1.1.3 (2021-06-11) ------------------- -* Merge branch 'melodic-2.8' into noetic -* Update BMSData msg to MiR software 2.8.3.1 -* Remove MirStatus - This message was removed in MiR software 2.0 (Renamed to RobotStatus). -* Update mir_msgs to 2.8.2.2 -* Contributors: Felix, Martin Günther - -1.1.2 (2021-05-12) ------------------- - 1.1.1 (2021-02-11) ------------------ * Contributors: Martin Günther diff --git a/mir_msgs/CMakeLists.txt b/mir_msgs/CMakeLists.txt index e77fb898..8b537cec 100644 --- a/mir_msgs/CMakeLists.txt +++ b/mir_msgs/CMakeLists.txt @@ -1,77 +1,77 @@ -cmake_minimum_required(VERSION 3.5.1) -cmake_policy(SET CMP0048 NEW) + +cmake_minimum_required(VERSION 3.5) project(mir_msgs) -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - message_generation -) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -################################################ -## Declare ROS messages, services and actions ## -################################################ +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -# Generate messages in the 'msg' folder -add_message_files( - FILES - BatteryCurrents.msg - BMSData.msg - BrakeState.msg - ChargingState.msg - Device.msg - Devices.msg - Encoders.msg - Error.msg - Event.msg - Events.msg - ExternalRobot.msg - ExternalRobots.msg - Gpio.msg - GripperState.msg - HeightState.msg - HookExtendedStatus.msg - HookStatus.msg - IOs.msg - JoystickVel.msg - LocalMapStat.msg - MirExtra.msg - MissionCtrlCommand.msg - MissionCtrlState.msg - PalletLifterStatus.msg - Path.msg - Pendant.msg - PlanSegment.msg - PlanSegments.msg - Pose2D.msg - PowerBoardMotorStatus.msg - Proximity.msg - ResourcesState.msg - ResourceState.msg - RobotMode.msg - RobotState.msg - RobotStatus.msg - SafetyStatus.msg - Serial.msg - StampedEncoders.msg - Trolley.msg - Twist2D.msg - UserPrompt.msg - WebPath.msg - WorldMap.msg - WorldModel.msg -) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) -# Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - geometry_msgs +set(msg_files + "msg/BatteryCurrents.msg" + "msg/BMSData.msg" + # "msg/BrakeState.msg" + # "msg/ChargingState.msg" + # "msg/Device.msg" + # "msg/Devices.msg" + # "msg/Encoders.msg" + # "msg/Error.msg" + # "msg/Event.msg" + # "msg/Events.msg" + # "msg/ExternalRobot.msg" + # "msg/ExternalRobots.msg" + # "msg/Gpio.msg" + # "msg/GripperState.msg" + # "msg/HeightState.msg" + # "msg/HookExtendedStatus.msg" + # "msg/HookStatus.msg" + # "msg/IOs.msg" + # "msg/JoystickVel.msg" + # "msg/LocalMapStat.msg" + # "msg/MirExtra.msg" + # "msg/MirStatus.msg" + # "msg/MissionCtrlCommand.msg" + # "msg/MissionCtrlState.msg" + # "msg/PalletLifterStatus.msg" + # "msg/Path.msg" + # "msg/Pendant.msg" + # "msg/PlanSegment.msg" + # "msg/PlanSegments.msg" + # "msg/Pose2D.msg" + # "msg/PowerBoardMotorStatus.msg" + # "msg/Proximity.msg" + # "msg/ResourcesState.msg" + # "msg/ResourceState.msg" + # "msg/RobotMode.msg" + # "msg/RobotState.msg" + # "msg/RobotStatus.msg" + # "msg/SafetyStatus.msg" + # "msg/Serial.msg" + # "msg/StampedEncoders.msg" + # "msg/Trolley.msg" + # "msg/Twist2D.msg" + # "msg/UserPrompt.msg" + # "msg/WebPath.msg" + # "msg/WorldMap.msg" + # "msg/WorldModel.msg" + "srv/ExecMission.srv" ) -################################### -## catkin specific configuration ## -################################### -catkin_package( - CATKIN_DEPENDS - geometry_msgs - message_runtime +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES builtin_interfaces geometry_msgs ) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/mir_msgs/msg/BMSData.msg b/mir_msgs/msg/BMSData.msg index 486fe27d..71d371bf 100644 --- a/mir_msgs/msg/BMSData.msg +++ b/mir_msgs/msg/BMSData.msg @@ -5,69 +5,10 @@ int32 state_of_charge float64 remaining_time_to_full_charge int32 remaining_capacity int32 state_of_health -int32 DISCHARGING=1 #bit 0 -int32 CHARGING=2 #bit 1 -int32 OV=4 #bit 2 Over voltage -int32 UV=8 #bit 3 Under voltage -int32 COC=16 #bit 4 Charge over current -int32 DOC=32 #bit 5 Discharge over current -int32 DOT=64 #bit 6 Discharge over temperature -int32 DUT=128 #bit 7 Discharge under temperature -int32 SC=512 #bit 9 -int32 COT=1024 #bit 10 Charge over temperature -int32 CUT=2048 #bit 11 Charge under temperature -int32 FW_STATUS_MSK=2031616 # to get Battery_Firmware_Status do the following: -int32 FW_STATUS_SHIFT=16 # batt_fw_stat=(status_flags & FW_STATUS_MSK)>>FW_STATUS_SHIFT -int32 FW_UPD_OK=0 #Battery firmware update finished OK. -int32 FW_UPD_RUNNING=1 #Battery firmware update running. -int32 FW_UPD_FAILED_BOOT=2 #Battery firmware update failed in Bootloader (Robot must not drive) -int32 FW_UPD_FAILED_APP=3 #Battery firmware update failed updating the application (Robot can drive with old FW) -int32 FW_UPD_FAILED_PARAM=4 #Battery firmware update failed uploading parameters (Robot can drive with old fw and parameters.) -int32 FW_STATUS_LOW_BATT=5 #Battery firmware update skipped battery too low (Robot can drive with old parameters.) -int32 FW_STATUS_FILE_CORRUPTED=6 #Battery firmware file corrupted (Robot can drive with old parameters.) -int32 FW_STATUS_CURRENT_TO_HIGH=7 #Battery firmware file corrupted (Robot can drive with old parameters.) -int32 FW_STATUS_NO_CAN=8 #Battery firmware update skipped no CAN communication (Robot can drive with old fw and parameters.) -int32 FW_BATTERY_IMBALANCE_HIGH=9 #Battery firmware update is enforced and the battery will be shut off by the new firmware +int32 DISCHARGING=1 +int32 CHARGING=2 int32 status_flags int32 temperature -uint32[] cell_voltage # In Mk2 robots and above the BMS provides data for 8 battery cells. 2Gen robots have BMS for 13 battery cells +uint32[] cell_voltage # In Mk2 robots and above the BMS provides data for 8 battery cells. MiR500 robots have BMS for 13 battery cells -# Exteded diagnosticts for BMZ battery -uint32 bmz_flag # Flag for enabling extended diagnosticts -float64 full_voltage -int32 full_capacity -int32 temperature2 -int32 temperature3 -int32 cycle_count -int32 dsg_overcurrent_counter -int32 chg_overcurrent_counter -int32 hw_major -int32 hw_minor -int32 fw_major -int32 fw_minor -int32 fw_patch -int32 rec_fw_major -int32 rec_fw_minor -int32 bl_major -int32 bl_minor -uint32 status_enabled -uint32 status_current_limitation -uint32 status_switch_off_warn1 -uint32 status_switch_off_warn2 -uint32 status_fully_discharged -uint32 status_nearly_discharged -uint32 status_chargefet_on -uint32 status_dischargefet_on -uint32 status_discharging -uint32 status_fully_charged -uint32 status_charging -uint32 status_temp_charging_err -uint32 status_cell_over_voltage -uint32 status_cell_under_voltage -uint32 status_charge_over_current -uint32 status_shortcircuit -uint32 status_discharge_over_current -uint32 status_temp_discharging_err -uint32 status_charger_detected - -float64 last_battery_msg_time +float64 last_battery_msg_time \ No newline at end of file diff --git a/mir_msgs/msg/BatteryCurrents.msg b/mir_msgs/msg/BatteryCurrents.msg index d81b7dde..9e86f4b6 100644 --- a/mir_msgs/msg/BatteryCurrents.msg +++ b/mir_msgs/msg/BatteryCurrents.msg @@ -1,2 +1,2 @@ float64 battery1_current -float64 battery2_current +float64 battery2_current \ No newline at end of file diff --git a/mir_msgs/msg/BrakeState.msg b/mir_msgs/msg/BrakeState.msg index 22946279..b501bd7e 100644 --- a/mir_msgs/msg/BrakeState.msg +++ b/mir_msgs/msg/BrakeState.msg @@ -1,3 +1,3 @@ string state_string uint8 state -bool braked +bool braked \ No newline at end of file diff --git a/mir_msgs/msg/ChargingState.msg b/mir_msgs/msg/ChargingState.msg index c2b50be0..05ec09ea 100644 --- a/mir_msgs/msg/ChargingState.msg +++ b/mir_msgs/msg/ChargingState.msg @@ -7,3 +7,4 @@ float64 charging_voltage uint32 charging_voltage_raw bool is_voltage_low float64 last_time_voltage + diff --git a/mir_msgs/msg/EncoderTestEntry.msg b/mir_msgs/msg/EncoderTestEntry.msg deleted file mode 100644 index e8e8ba23..00000000 --- a/mir_msgs/msg/EncoderTestEntry.msg +++ /dev/null @@ -1,6 +0,0 @@ -float64 command_velocity -float64 command_distance -float64 left_dist -float64 right_dist -string suggested_direction -string user_direction diff --git a/mir_msgs/msg/Error.msg b/mir_msgs/msg/Error.msg index 6dae8f32..1b3df828 100644 --- a/mir_msgs/msg/Error.msg +++ b/mir_msgs/msg/Error.msg @@ -10,9 +10,6 @@ int32 MOTOR_ERROR = 700 int32 LASER_ERROR = 800 int32 CAMERA_ERROR = 900 int32 SAFETY_SYSTEM_ERROR = 1000 -int32 POWERBOARD_ERROR = 2000 -int32 POWERSUPPLY_ERROR = 2100 -int32 CANBUS_ERROR = 2200 int32 HOOK_ERROR = 5000 int32 HOOK_CAMERA_ERROR = 5100 int32 HOOK_ACTUATOR_ERROR = 5200 @@ -26,7 +23,7 @@ int32 MAPPING_ERROR = 10300 int32 ODOM_FUSION_ERROR = 10400 -time timestamp # Timestamp for when the error occurred +builtin_interfaces/Time timestamp # Timestamp for when the error occurred int32 code # Error code string description # Error description string module # Module in which the error occurred diff --git a/mir_msgs/msg/Event.msg b/mir_msgs/msg/Event.msg index a183c83a..c6aaa910 100644 --- a/mir_msgs/msg/Event.msg +++ b/mir_msgs/msg/Event.msg @@ -7,7 +7,7 @@ uint32 EV_FWDDIST=6 uint32 EV_IO=7 uint32 EV_FLEETLCK=8 # Fleet uint32 EV_EMERGENCY=9 # Fleet -uint32 eventType # The area event type +uint32 event_type # The area event type string area_guid # The area unique identifier string area_name # The name of the area -geometry_msgs/Point[] polygon # An array of corner points that define the edges of the area +geometry_msgs/Point[] polygon # An array of corner points that define the edges of the area \ No newline at end of file diff --git a/mir_msgs/msg/Events.msg b/mir_msgs/msg/Events.msg index 770319d8..d52cfb3f 100644 --- a/mir_msgs/msg/Events.msg +++ b/mir_msgs/msg/Events.msg @@ -1,2 +1,2 @@ Header header -Event[] events +Event[] events \ No newline at end of file diff --git a/mir_msgs/msg/ExternalRobot.msg b/mir_msgs/msg/ExternalRobot.msg index 8997d715..09d6cd7f 100644 --- a/mir_msgs/msg/ExternalRobot.msg +++ b/mir_msgs/msg/ExternalRobot.msg @@ -1,11 +1,6 @@ Header header uint32 id -uint32 MIR100=1 -uint32 MIR500=3 -uint32 type_id string name -float64 robot_length -float64 robot_width string footprint string ip uint32 map_id diff --git a/mir_msgs/msg/GripperState.msg b/mir_msgs/msg/GripperState.msg index dcca3fc9..4bcf5f05 100644 --- a/mir_msgs/msg/GripperState.msg +++ b/mir_msgs/msg/GripperState.msg @@ -1,3 +1,3 @@ string state_string uint8 state -bool closed +bool closed \ No newline at end of file diff --git a/mir_msgs/msg/HeightState.msg b/mir_msgs/msg/HeightState.msg index 5efa1088..ba47bd1f 100644 --- a/mir_msgs/msg/HeightState.msg +++ b/mir_msgs/msg/HeightState.msg @@ -1,3 +1,3 @@ string state_string uint8 state -float64 height +float64 height \ No newline at end of file diff --git a/mir_msgs/msg/IOs.msg b/mir_msgs/msg/IOs.msg index 5ef8fe59..d62eb4f7 100644 --- a/mir_msgs/msg/IOs.msg +++ b/mir_msgs/msg/IOs.msg @@ -9,4 +9,4 @@ bool[] input_state int8 num_outputs bool[] output_state string ip -string error +string error \ No newline at end of file diff --git a/mir_msgs/msg/JoystickVel.msg b/mir_msgs/msg/JoystickVel.msg index f905658d..7bbc9d84 100644 --- a/mir_msgs/msg/JoystickVel.msg +++ b/mir_msgs/msg/JoystickVel.msg @@ -1,2 +1,2 @@ string joystick_token -geometry_msgs/Twist speed_command +geometry_msgs/Twist speed_command \ No newline at end of file diff --git a/mir_msgs/msg/LocalMapStat.msg b/mir_msgs/msg/LocalMapStat.msg index 4d216c0b..c95e6703 100644 --- a/mir_msgs/msg/LocalMapStat.msg +++ b/mir_msgs/msg/LocalMapStat.msg @@ -1,3 +1,4 @@ int32 idx int32 x int32 y + diff --git a/mir_msgs/msg/MirStatus.msg b/mir_msgs/msg/MirStatus.msg new file mode 100644 index 00000000..d4a6706a --- /dev/null +++ b/mir_msgs/msg/MirStatus.msg @@ -0,0 +1,10 @@ +# MirStatus - to publish data on a topic +int32 state # system state +string mode # overall operation of the robot +string msg # status message +float32 uptime # time since start in min +float32 moved # combined distance moved since last start +float32 battery # voltage of battery +float32 battery_percentage # battery left in percent +int32 battery_time_left # battery time left in seconds +float32 eta # estimated time of arrival (not impl) diff --git a/mir_msgs/msg/PlanSegment.msg b/mir_msgs/msg/PlanSegment.msg index 675fdb54..07e7ca67 100644 --- a/mir_msgs/msg/PlanSegment.msg +++ b/mir_msgs/msg/PlanSegment.msg @@ -2,4 +2,4 @@ bool forward_motion int32 start_idx float64 length float64 remaining_length -geometry_msgs/PoseStamped[] path +geometry_msgs/PoseStamped[] path \ No newline at end of file diff --git a/mir_msgs/msg/PlanSegments.msg b/mir_msgs/msg/PlanSegments.msg index 73e62c98..330c50ea 100644 --- a/mir_msgs/msg/PlanSegments.msg +++ b/mir_msgs/msg/PlanSegments.msg @@ -1 +1 @@ -mir_msgs/PlanSegment[] p_segments +mir_msgs/PlanSegment[] p_segments \ No newline at end of file diff --git a/mir_msgs/msg/PowerBoardMotorStatus.msg b/mir_msgs/msg/PowerBoardMotorStatus.msg index f87472fa..45d39a65 100644 --- a/mir_msgs/msg/PowerBoardMotorStatus.msg +++ b/mir_msgs/msg/PowerBoardMotorStatus.msg @@ -1,24 +1,24 @@ -uint16 LeftMotor_CtrlWord -int32 LeftMotor_Speed -int32 LeftMotor_Encoder -uint16 LeftMotor_Status -uint8 LeftMotor_Error -uint32 LeftMotor_ErrorHist1 -uint32 LeftMotor_ErrorHist2 -int32 LeftMotor_Current -uint16 LeftMotor_I2t_Motor -uint16 LeftMotor_I2t_Controller -int16 LeftMotor_Temperature -uint16 RightMotor_CtrlWord -int32 RightMotor_Speed -int32 RightMotor_Encoder -uint16 RightMotor_Status -uint8 RightMotor_Error -uint32 RightMotor_ErrorHist1 -uint32 RightMotor_ErrorHist2 -int32 RightMotor_Current -uint16 RightMotor_I2t_Motor -uint16 RightMotor_I2t_Controller -int16 RightMotor_Temperature -uint8 Brake_LeftStatus -uint8 Brake_RightStatus +uint16 left_motor_ctrl_word +int32 left_motor_speed +int32 left_motor_encoder +uint16 left_motor_status +uint8 left_motor_error +uint32 left_motor_error_hist1 +uint32 left_motor_error_hist2 +int32 left_motor_current +uint16 left_motor_i2t_motor +uint16 left_motor_i2t_controller +int16 left_motor_temperature +uint16 right_motor_ctrl_word +int32 right_motor_speed +int32 right_motor_encoder +uint16 right_motor_status +uint8 right_motor_error +uint32 right_motor_error_hist1 +uint32 right_motor_error_hist2 +int32 right_motor_current +uint16 right_motor_i2t_motor +uint16 right_motor_i2t_controller +int16 right_motor_temperature +uint8 brake_left_status +uint8 brake_right_status \ No newline at end of file diff --git a/mir_msgs/msg/PrecisionDockingStatus.msg b/mir_msgs/msg/PrecisionDockingStatus.msg deleted file mode 100644 index db614f87..00000000 --- a/mir_msgs/msg/PrecisionDockingStatus.msg +++ /dev/null @@ -1,5 +0,0 @@ -bool connected -bool motor_forward -bool motor_back -bool left_docking -bool right_docking diff --git a/mir_msgs/msg/ResourceState.msg b/mir_msgs/msg/ResourceState.msg index fd928ce0..9ef12d42 100644 --- a/mir_msgs/msg/ResourceState.msg +++ b/mir_msgs/msg/ResourceState.msg @@ -1,12 +1,12 @@ string[] assigned # A token that is true whenever the resource is busy. -uint32 ROBOT_POSITION=0 +uint32 ROBOT_POSITION=0 uint32 STAGING_POSITION=1 uint32 CHARGING_STATION=2 uint32 AREA=3 uint32 type # The resource type uint32 path_idx # The index from the global path in which the robot gets into the position float32 distance # The distance from the robot to the resource -geometry_msgs/Point collision_point # The collision point with the resource +geometry_msgs/Point collision_point # The collision point with the resource string[] queue # The queue for a resource. It's a list of robots ips. string name # The name of the resource string guid # The guid of the resource diff --git a/mir_msgs/msg/ResourcesState.msg b/mir_msgs/msg/ResourcesState.msg index 97b44624..73efc460 100644 --- a/mir_msgs/msg/ResourcesState.msg +++ b/mir_msgs/msg/ResourcesState.msg @@ -1,2 +1,2 @@ Header header -ResourceState[] resources +ResourceState[] resources \ No newline at end of file diff --git a/mir_msgs/msg/RobotMode.msg b/mir_msgs/msg/RobotMode.msg index 34cbb3a2..d3364d64 100644 --- a/mir_msgs/msg/RobotMode.msg +++ b/mir_msgs/msg/RobotMode.msg @@ -1,9 +1,9 @@ # The robot operates in different mode uint8 ROBOT_MODE_NONE = 0 # start mode uint8 ROBOT_MODE_MAPPING = 3 # in mapping a new map is made -uint8 ROBOT_MODE_MAPPING_FINALIZING = 4 # in mapping the recorded map is being finalised uint8 ROBOT_MODE_MISSION = 7 # primary mode when executing a mission (action list) uint8 ROBOT_MODE_CHANGING = 255 # a transition mode - to say that a transition is in progress -uint8 robotMode -string robotModeString +uint8 robot_mode +string robot_mode_string + diff --git a/mir_msgs/msg/RobotState.msg b/mir_msgs/msg/RobotState.msg index f78c555b..fb20bfb7 100644 --- a/mir_msgs/msg/RobotState.msg +++ b/mir_msgs/msg/RobotState.msg @@ -1,11 +1,11 @@ -# The robot has to be in a predefined state +# The robot has to be in a predefined state uint8 ROBOT_STATE_NONE = 0 uint8 ROBOT_STATE_STARTING = 1 uint8 ROBOT_STATE_SHUTTINGDOWN = 2 uint8 ROBOT_STATE_READY = 3 # ready to execute uint8 ROBOT_STATE_PAUSE = 4 # pause from executing uint8 ROBOT_STATE_EXECUTING = 5 # when running in mission/taxa/bus -uint8 ROBOT_STATE_ABORTED = 6 +uint8 ROBOT_STATE_ABORTED = 6 uint8 ROBOT_STATE_COMPLETED = 7 # done executing uint8 ROBOT_STATE_DOCKED = 8 # in the dock and charging the batteries uint8 ROBOT_STATE_DOCKING = 9 @@ -13,5 +13,6 @@ uint8 ROBOT_STATE_EMERGENCYSTOP = 10 # the robot has emg-stop activated uint8 ROBOT_STATE_MANUALCONTROL = 11 # a pause state, where the robot can move uint8 ROBOT_STATE_ERROR = 12 # a general error state, requires a error handle -uint8 robotState -string robotStateString +uint8 robot_state +string robot_state_string + diff --git a/mir_msgs/msg/RobotStatus.msg b/mir_msgs/msg/RobotStatus.msg index d183e7c6..5f308aa4 100644 --- a/mir_msgs/msg/RobotStatus.msg +++ b/mir_msgs/msg/RobotStatus.msg @@ -21,7 +21,3 @@ string state_text int32 uptime Twist2D velocity mir_msgs/UserPrompt user_prompt -bool safety_system_muted -bool joystick_low_speed_mode_enabled -string joystick_web_session_id -string mode_key_state diff --git a/mir_msgs/msg/SafetyStatus.msg b/mir_msgs/msg/SafetyStatus.msg index 15951949..5c05d9c1 100644 --- a/mir_msgs/msg/SafetyStatus.msg +++ b/mir_msgs/msg/SafetyStatus.msg @@ -9,32 +9,8 @@ bool sto_feedback bool is_restart_required bool is_safety_muted -float64 max_lin_speed -float64 max_rot_speed - -# Defines for filling out the mute_mask -uint8 MUTE_FRONT_RIGHT = 1 -uint8 MUTE_FRONT_CENTER = 2 -uint8 MUTE_FRONT_LEFT = 4 -uint8 MUTE_LEFT_CENTER = 8 -uint8 MUTE_REAR_LEFT = 16 -uint8 MUTE_REAR_CENTER = 32 -uint8 MUTE_REAR_RIGHT = 64 -uint8 MUTE_RIGHT_CENTER = 128 - -uint8 MUTE_FRONT = 7 -uint8 MUTE_LEFT = 28 -uint8 MUTE_REAR = 112 -uint8 MUTE_RIGHT = 193 -uint8 MUTE_SIDES = 221 -uint8 MUTE_ALL = 255 - -uint8 mute_mask -uint8 partial_mute_mask - bool is_limited_speed_active -bool is_lifter_down bool in_sleep_mode bool in_manual_mode -bool is_manual_mode_restart_required +bool is_manual_mode_restart_required \ No newline at end of file diff --git a/mir_msgs/msg/Serial.msg b/mir_msgs/msg/Serial.msg index 9741bdaf..108129bd 100644 --- a/mir_msgs/msg/Serial.msg +++ b/mir_msgs/msg/Serial.msg @@ -1,2 +1,3 @@ Header header string data + diff --git a/mir_msgs/msg/SoundEvent.msg b/mir_msgs/msg/SoundEvent.msg deleted file mode 100644 index 73b21ef0..00000000 --- a/mir_msgs/msg/SoundEvent.msg +++ /dev/null @@ -1,16 +0,0 @@ -time time_stamp -string sound_guid -string message - -uint8 START=0 -uint8 STOP =1 -uint8 MUTE=2 -uint8 UNMUTE=3 -uint8 PAUSE=4 -uint8 UNPAUSE=5 -uint8 FINISH=6 -uint8 MUTEABLE=7 -uint8 REQ_PLAY=10 - - -uint8 event diff --git a/mir_msgs/msg/StampedEncoders.msg b/mir_msgs/msg/StampedEncoders.msg index ff8193c9..3524e6a9 100644 --- a/mir_msgs/msg/StampedEncoders.msg +++ b/mir_msgs/msg/StampedEncoders.msg @@ -1,2 +1,2 @@ Header header -Encoders encoders +Encoders encoders \ No newline at end of file diff --git a/mir_msgs/msg/UserPrompt.msg b/mir_msgs/msg/UserPrompt.msg index b3e2c45d..04dd88fe 100644 --- a/mir_msgs/msg/UserPrompt.msg +++ b/mir_msgs/msg/UserPrompt.msg @@ -3,4 +3,4 @@ string guid string user_group string question string[] options -duration timeout +builtin_interfaces/Duration timeout \ No newline at end of file diff --git a/mir_msgs/msg/WorldMap.msg b/mir_msgs/msg/WorldMap.msg index 2716ccb0..58cc6033 100644 --- a/mir_msgs/msg/WorldMap.msg +++ b/mir_msgs/msg/WorldMap.msg @@ -1,4 +1,4 @@ mir_msgs/ResourcesState positions mir_msgs/ResourcesState areas mir_msgs/ExternalRobots robots -int32 map_id +int32 map_id \ No newline at end of file diff --git a/mir_msgs/msg/WorldModel.msg b/mir_msgs/msg/WorldModel.msg index f920b985..8ecbfbe3 100644 --- a/mir_msgs/msg/WorldModel.msg +++ b/mir_msgs/msg/WorldModel.msg @@ -1,2 +1,2 @@ Header header -mir_msgs/WorldMap[] world_map # world model for a particular map +mir_msgs/WorldMap[] world_map # world model for a particular map \ No newline at end of file diff --git a/mir_msgs/package.xml b/mir_msgs/package.xml index 13798802..d413256f 100644 --- a/mir_msgs/package.xml +++ b/mir_msgs/package.xml @@ -1,7 +1,7 @@ - + mir_msgs - 1.1.3 + 1.1.1 Message definitions for the MiR100 robot Martin Günther @@ -13,10 +13,16 @@ https://github.com/dfki-ric/mir_robot https://github.com/dfki-ric/mir_robot/issues - catkin + ament_cmake + builtin_interfaces + rosidl_default_generators geometry_msgs - message_generation - message_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/mir_msgs/srv/ExecMission.srv b/mir_msgs/srv/ExecMission.srv new file mode 100644 index 00000000..74d9d768 --- /dev/null +++ b/mir_msgs/srv/ExecMission.srv @@ -0,0 +1,4 @@ +string mission_name +--- +string message +bool success \ No newline at end of file diff --git a/mir_navigation/CMakeLists.txt b/mir_navigation/CMakeLists.txt index 9a65c215..8b7067a8 100644 --- a/mir_navigation/CMakeLists.txt +++ b/mir_navigation/CMakeLists.txt @@ -1,67 +1,35 @@ -cmake_minimum_required(VERSION 3.5.1) -cmake_policy(SET CMP0048 NEW) +cmake_minimum_required(VERSION 3.5) project(mir_navigation) -find_package(catkin REQUIRED COMPONENTS - roslaunch -) - -################################### -## catkin specific configuration ## -################################### -catkin_package() - -############# -## Install ## -############# +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -# Mark executable scripts (Python etc.) for installation -# in contrast to setup.py, you can choose the destination -install(PROGRAMS - mprim/genmprim_unicycle_highcost_5cm.py - nodes/acc_finder.py - nodes/min_max_finder.py - scripts/plot_mprim.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Mark executables and/or libraries for installation -# install(TARGETS mir_navigation mir_navigation_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(mir_gazebo) +find_package(nav2_bringup) +find_package(rviz2) -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) -# Mark other files for installation (e.g. launch and bag files, etc.) install(DIRECTORY - config - launch - mprim - rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + config launch rviz maps behavior_trees + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_mir_navigation.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() -roslaunch_add_file_check(launch) +ament_package() \ No newline at end of file diff --git a/mir_navigation/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/mir_navigation/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml new file mode 100644 index 00000000..324b9cc9 --- /dev/null +++ b/mir_navigation/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mir_navigation/config/base_local_planner_params.yaml b/mir_navigation/config/base_local_planner_params.yaml deleted file mode 100644 index ac2fba90..00000000 --- a/mir_navigation/config/base_local_planner_params.yaml +++ /dev/null @@ -1,44 +0,0 @@ -base_local_planner: base_local_planner/TrajectoryPlannerROS -TrajectoryPlannerROS: - # Robot configuration - acc_lim_x: 1.5 - acc_lim_y: 0 - acc_lim_theta: 2.0 - max_vel_x: 0.8 - min_vel_x: 0.1 - max_vel_theta: 1.0 - min_vel_theta: -1.0 - min_in_place_vel_theta: 0.2 - escape_vel: -0.1 - holonomic_robot: false - - # Goal tolerance - yaw_goal_tolerance: 0.03 - xy_goal_tolerance: 0.08 - latch_xy_goal_tolerance: true - - # Forward simulaton parameters - sim_time: 1.2 - sim_granularity: 0.025 - angular_sim_granularity: 0.04 - vx_samples: 15 - vtheta_samples: 20 - controller_frequency: 5 - - # Trajectory scoring parameters - meter_scoring: true - pdist_scale: 2.2 #0.6 - gdist_scale: 0.8 #0.8 - occdist_scale: 0.1 - heading_lookahead: 0.325 - heading_scoring: true - heading_scoring_timestep: 0.8 - dwa: true - publish_cost_grid: false - global_frame_id: map - - # Oscillation prevention parameter - oscillation_reset_dist: 0.05 - - # Global plan parameters - prune_plan: true diff --git a/mir_navigation/config/costmap_common_params.yaml b/mir_navigation/config/costmap_common_params.yaml deleted file mode 100644 index 90fc3509..00000000 --- a/mir_navigation/config/costmap_common_params.yaml +++ /dev/null @@ -1,46 +0,0 @@ -robot_base_frame: $(arg prefix)base_footprint -transform_tolerance: 0.4 -update_frequency: 5.0 -publish_frequency: 1.0 -obstacle_range: 3.0 -#mark_threshold: 1 -publish_voxel_map: true -navigation_map: - map_topic: /map -obstacles: - observation_sources: b_scan_marking b_scan_clearing f_scan_marking f_scan_clearing - b_scan_marking: - topic: b_scan_rep117 - data_type: LaserScan - clearing: false - marking: true - inf_is_valid: false - min_obstacle_height: 0.13 - max_obstacle_height: 0.25 - b_scan_clearing: - topic: b_scan_rep117 - data_type: LaserScan - clearing: true - marking: false - inf_is_valid: false - min_obstacle_height: 0.13 - max_obstacle_height: 0.25 - f_scan_marking: - topic: f_scan_rep117 - data_type: LaserScan - clearing: false - marking: true - inf_is_valid: false - min_obstacle_height: 0.13 - max_obstacle_height: 0.25 - f_scan_clearing: - topic: f_scan_rep117 - data_type: LaserScan - clearing: true - marking: false - inf_is_valid: false - min_obstacle_height: 0.13 - max_obstacle_height: 0.25 -virtual_walls_map: - map_topic: /virtual_walls/map - use_maximum: true diff --git a/mir_navigation/config/costmap_global_params.yaml b/mir_navigation/config/costmap_global_params.yaml deleted file mode 100644 index 5b927d7d..00000000 --- a/mir_navigation/config/costmap_global_params.yaml +++ /dev/null @@ -1,14 +0,0 @@ -global_costmap: - global_frame: map - static_map: true - update_frequency: 1.0 - publish_frequency: 1.0 - raytrace_range: 2.0 - resolution: 0.05 - z_resolution: 0.2 - z_voxels: 10 - inflation: - cost_scaling_factor: 3.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. - inflation_radius: 0.6 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. - - # plugins are loaded via costmap_global_params_plugins_[mapping|planning].yaml diff --git a/mir_navigation/config/costmap_global_params_plugins_no_virtual_walls.yaml b/mir_navigation/config/costmap_global_params_plugins_no_virtual_walls.yaml deleted file mode 100644 index 082af167..00000000 --- a/mir_navigation/config/costmap_global_params_plugins_no_virtual_walls.yaml +++ /dev/null @@ -1,5 +0,0 @@ -global_costmap: - plugins: - - {name: navigation_map, type: "costmap_2d::StaticLayer" } - - {name: obstacles, type: "costmap_2d::VoxelLayer" } - - {name: inflation, type: "costmap_2d::InflationLayer" } diff --git a/mir_navigation/config/costmap_global_params_plugins_virtual_walls.yaml b/mir_navigation/config/costmap_global_params_plugins_virtual_walls.yaml deleted file mode 100644 index 86087ea4..00000000 --- a/mir_navigation/config/costmap_global_params_plugins_virtual_walls.yaml +++ /dev/null @@ -1,6 +0,0 @@ -global_costmap: - plugins: - - {name: navigation_map, type: "costmap_2d::StaticLayer" } - - {name: obstacles, type: "costmap_2d::VoxelLayer" } - - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" } - - {name: inflation, type: "costmap_2d::InflationLayer" } diff --git a/mir_navigation/config/costmap_local_params.yaml b/mir_navigation/config/costmap_local_params.yaml deleted file mode 100644 index 894c0939..00000000 --- a/mir_navigation/config/costmap_local_params.yaml +++ /dev/null @@ -1,17 +0,0 @@ -local_costmap: - global_frame: $(arg prefix)odom - static_map: false - rolling_window: true - raytrace_range: 6.0 - resolution: 0.05 - z_resolution: 0.15 - z_voxels: 8 - inflation: - cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. - inflation_radius: 0.6 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. - width: 4.0 - height: 4.0 - origin_x: 0.0 - origin_y: 0.0 - - # plugins are loaded via costmap_local_params_plugins_[mapping|planning].yaml diff --git a/mir_navigation/config/costmap_local_params_plugins_no_virtual_walls.yaml b/mir_navigation/config/costmap_local_params_plugins_no_virtual_walls.yaml deleted file mode 100644 index 6a287336..00000000 --- a/mir_navigation/config/costmap_local_params_plugins_no_virtual_walls.yaml +++ /dev/null @@ -1,4 +0,0 @@ -local_costmap: - plugins: - - {name: obstacles, type: "costmap_2d::VoxelLayer" } - - {name: inflation, type: "costmap_2d::InflationLayer" } diff --git a/mir_navigation/config/costmap_local_params_plugins_virtual_walls.yaml b/mir_navigation/config/costmap_local_params_plugins_virtual_walls.yaml deleted file mode 100644 index bc8a0cba..00000000 --- a/mir_navigation/config/costmap_local_params_plugins_virtual_walls.yaml +++ /dev/null @@ -1,5 +0,0 @@ -local_costmap: - plugins: - - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" } - - {name: obstacles, type: "costmap_2d::VoxelLayer" } - - {name: inflation, type: "costmap_2d::InflationLayer" } diff --git a/mir_navigation/config/dwa_local_planner_params.yaml b/mir_navigation/config/dwa_local_planner_params.yaml deleted file mode 100644 index b8d766af..00000000 --- a/mir_navigation/config/dwa_local_planner_params.yaml +++ /dev/null @@ -1,55 +0,0 @@ -base_local_planner: dwa_local_planner/DWAPlannerROS -DWAPlannerROS: - # Robot configuration - max_vel_x: 0.8 - min_vel_x: -0.2 - - max_vel_y: 0.0 # diff drive robot - min_vel_y: 0.0 # diff drive robot - - max_trans_vel: 0.8 # choose slightly less than the base's capability - min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity - trans_stopped_vel: 0.03 - - # Warning! - # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities - # are non-negligible and small in place rotational velocities will be created. - - max_rot_vel: 1.0 # choose slightly less than the base's capability - min_rot_vel: 0.1 # this is the min angular velocity when there is negligible translational velocity - rot_stopped_vel: 0.1 - - acc_lim_x: 1.5 - acc_lim_y: 0.0 # diff drive robot - acc_limit_trans: 1.5 - acc_lim_theta: 2.0 - - # Goal tolerance - yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide) - xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2 - latch_xy_goal_tolerance: true - - # Forward simulation - sim_time: 1.2 - vx_samples: 15 - vy_samples: 1 # diff drive robot, there is only one sample - vtheta_samples: 20 - - # Trajectory scoring - path_distance_bias: 64.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan - goal_distance_bias: 12.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal - occdist_scale: 0.5 # default: 0.01 mir: 0.01 - weighting for how much the controller should avoid obstacles - forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point - stop_time_buffer: 0.2 # default: 0.2 mir: 0.2 - amount of time a robot must stop before colliding for a valid traj. - scaling_speed: 0.25 # default: 0.25 mir: 0.25 - absolute velocity at which to start scaling the robot's footprint - max_scaling_factor: 0.2 # default: 0.2 mir: 0.2 - how much to scale the robot's footprint when at speed. - prune_plan: true - - # Oscillation prevention - oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags, in m - oscillation_reset_angle: 0.2 # 0.2 - the angle the robot must turn before resetting Oscillation flags, in rad - - # Debugging - publish_traj_pc : true - publish_cost_grid_pc: true - global_frame_id: /odom # or /odom diff --git a/mir_navigation/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml deleted file mode 100644 index d59aacb5..00000000 --- a/mir_navigation/config/dwb_local_planner_params.yaml +++ /dev/null @@ -1,92 +0,0 @@ -base_local_planner: nav_core_adapter::LocalPlannerAdapter -LocalPlannerAdapter: - planner_name: dwb_local_planner::DWBLocalPlanner -DWBLocalPlanner: - # Robot configuration - max_vel_x: 0.8 - min_vel_x: -0.2 - - max_vel_y: 0.0 # diff drive robot - min_vel_y: 0.0 # diff drive robot - - max_speed_xy: 0.8 # max_trans_vel: 0.8 # choose slightly less than the base's capability - min_speed_xy: 0.1 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity - - max_vel_theta: 1.0 # max_rot_vel: 1.0 # choose slightly less than the base's capability - min_speed_theta: 0.1 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity - - acc_lim_x: 1.5 - acc_lim_y: 0.0 # diff drive robot - acc_lim_theta: 2.0 - decel_lim_x: -1.5 - decel_lim_y: -0.0 - decel_lim_theta: -2.0 - - # Goal tolerance - yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide) - xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2 - #latch_xy_goal_tolerance: true - - # Whether to split the path into segments or not - # Requires https://github.com/locusrobotics/robot_navigation/pull/50 - split_path: true - - # Forward simulation (trajectory generation) - trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator # or dwb_plugins::LimitedAccelGenerator - sim_time: 0.8 - vx_samples: 15 - vy_samples: 1 # diff drive robot, there is only one sample - vtheta_samples: 15 - discretize_by_time: false - angular_granularity: 0.15 - linear_granularity: 0.05 - # sim_period - # include_last_point - - # Goal checking - goal_checker_name: dwb_plugins::SimpleGoalChecker - # stateful: true - - # Critics (trajectory scoring) - #default_critic_namespaces: [dwb_critics, mir_dwb_critics] - critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathProgress] - RotateToGoal: - scale: 100.0 - # lookahead_time: -1.0 - # slowing_factor: 5.0 - ObstacleFootprint: - scale: 0.01 # default: 0.01 mir: 0.01 - weighting for how much the controller should avoid obstacles - max_scaling_factor: 0.2 # default: 0.2 mir: 0.2 - how much to scale the robot's footprint when at speed. - scaling_speed: 0.25 # default: 0.25 mir: 0.25 - absolute velocity at which to start scaling the robot's footprint - sum_scores: false # if true, return sum of scores of all trajectory points instead of only last one - PathAlign: - scale: 16.0 - forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point - PathDistPruned: - scale: 32.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan - class: 'mir_dwb_critics::PathDistPruned' - PathProgress: - scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal - heading_scale: 0.1 - class: 'mir_dwb_critics::PathProgress' - xy_local_goal_tolerance: 0.20 - angle_threshold: 0.78539816 # 45 degrees - - - # Prune already passed poses from plan - prune_plan: true - prune_distance: 1.0 # Old poses farther away than prune_distance (in m) will be pruned. - # If the robot ever gets away further than this distance from the plan, - # the error "Resulting plan has 0 poses in it" will be thrown and - # replanning will be triggered. - - # Debugging - publish_cost_grid_pc: true - debug_trajectory_details: false - publish_evaluation: true - publish_global_plan: true - publish_input_params: true - publish_local_plan: true - publish_trajectories: true - publish_transformed_plan: true - marker_lifetime: 0.5 diff --git a/mir_navigation/config/eband_local_planner_params.yaml b/mir_navigation/config/eband_local_planner_params.yaml deleted file mode 100644 index 148af01d..00000000 --- a/mir_navigation/config/eband_local_planner_params.yaml +++ /dev/null @@ -1,44 +0,0 @@ -base_local_planner: eband_local_planner/EBandPlannerROS -EBandPlannerROS: - # Robot configuration - max_vel_lin: 0.8 # choose slightly less than the base's capability - min_vel_lin: 0.1 # this is the min trans velocity when there is negligible rotational velocity - trans_stopped_vel: 0.03 - - max_vel_th: 1.0 # choose slightly less than the base's capability - min_vel_th: 0.1 # this is the min angular velocity when there is negligible translational velocity - rot_stopped_vel: 0.1 - - min_in_place_vel_th: 0.1 # Minimum in-place angular velocity - in_place_trans_vel: 0.0 # Minimum in place linear velocity - - max_acceleration: 1.5 # Maximum allowable acceleration - max_translational_acceleration: 1.5 # Maximum linear acceleration - max_rotational_acceleration: 2.0 # Maximum angular acceleration - - # Goal tolerance - yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide) - xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2 - - # Elastic Band Parameters - eband_min_relative_overlap: 0.7 # Min distance that denotes connectivity between consecutive bubbles - eband_tiny_bubble_distance: 0.01 # Bubble geometric bound regarding tiny bubble distance - eband_tiny_bubble_expansion: 0.01 # Bubble geometric bound regarding tiny bubble expansion - eband_internal_force_gain: 1.0 # Force gain of forces between consecutive bubbles that tend to stretch the elastic band - eband_external_force_gain: 2.0 # Force gain of forces that tend to move the bubbles away from obstacles - num_iterations_eband_optimization: 3 # Number of iterations for eband optimization - eband_equilibrium_approx_max_recursion_depth: 4 # Number of iterations for reaching the equilibrium between internal and external forces - eband_equilibrium_relative_overshoot: 0.75 # Maximum relative equlibrium overshoot - eband_significant_force_lower_bound: 0.15 # Minimum magnitude of force that is considered significant and used in the calculations - costmap_weight: 10.0 # Costmap weight factor used in the calculation of distance to obstacles - - # Trajectory Controller Parameters - k_prop: 4.0 # Proportional gain of the PID controller - k_damp: 3.5 # Damping gain of the PID controller - Ctrl_Rate: 10.0 # Control rate - virtual_mass: 0.75 # Virtual mass - rotation_correction_threshold: 0.5 # Rotation correction threshold - differential_drive: True # Denotes whether to use the differential drive mode - bubble_velocity_multiplier: 2.0 # Multiplier of bubble radius - rotation_threshold_multiplier: 1.0 # Multiplier of rotation threshold - disallow_hysteresis: False # Determines whether to try getting closer to the goal, in case of going past the tolerance diff --git a/mir_navigation/config/mir_local_planner_params.yaml b/mir_navigation/config/mir_local_planner_params.yaml deleted file mode 100644 index ba7f82d6..00000000 --- a/mir_navigation/config/mir_local_planner_params.yaml +++ /dev/null @@ -1,81 +0,0 @@ -base_local_planner: mirLocalPlanner/MIRPlannerROS -MIRPlannerROS: - # Robot configuration - max_vel_x: 0.8 - min_vel_x: -0.2 - - min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity - - # Warning! - # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities - # are non-negligible and small in place rotational velocities will be created. - - max_rot_vel: 1.0 # choose slightly less than the base's capability - min_rot_vel: 0.1 # this is the min angular velocity when there is negligible translational velocity - - acc_lim_x: 1.5 - acc_lim_y: 0.0 # diff drive robot - acc_lim_th: 2.0 - min_inplace_rot: 0.15 - max_inplace_rot: 0.6 - min_in_place_rotational_vel: 0.2 - escape_vel: -0.1 - holonomic_robot: false - - # Goal tolerance - yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide) - xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2 - - # Forward simulaton - sim_time: 1.2 - sim_granularity: 0.025 - vx_samples: 15 - vth_samples: 20 - vtheta_samples: 20 - - # Trajectory scoring - path_distance_bias: 0.4 # weighting for how much it should stick to the global path plan - goal_distance_bias: 0.6 # weighting for how much it should attempt to reach its goal - occdist_scale: 0.01 # weighting for how much the controller should avoid obstacles - forward_point_distance: 0.325 # how far along to place an additional scoring point - stop_time_buffer: 0.2 # amount of time a robot must stop before colliding for a valid traj. - scaling_speed: 0.25 # absolute velocity at which to start scaling the robot's footprint - max_scaling_factor: 0.2 # how much to scale the robot's footprint when at speed. - heading_lookahead: 0.325 - dwa: true - - # Oscillation prevention - oscillation_reset_dist: 0.05 # how far to travel before resetting oscillation flags, in m - - - # Debugging - publish_visualization: false - publish_cost_grid_pc: false - global_frame_id: odom - - # MiR specific parameters - tau_p: 5.0 # The proportional value in the CTE PID tracking - tau_i: 0.1 # The integral value in the CTE PID tracking - tau_d: 0.5 # The differential value in the CTE PID tracking - tau_w: 9.0 # The proportional angle value in the CTE tracking - - k_rho: 1.0 # The proportional value to goal in Goal tracking - k_alfa: 8.0 # The diff angle between the robot and the goal in the Goal tracking - k_beta: -2.5 # The angle to the goal from the robot in the Goal tracking - - blocked_path_dist: 3.0 # At what distance should the planner react when the path is blocked - blocked_path_dev: 60.0 # How far can we move from the planned path when it is blocked - blocked_path_action: new_plan # Which action to take, when path is blocked - occ_path_dist: 3.0 # At what distance should the planner react when the path is near obstacle - occ_path_dev: 15.0 # How far can we move from the planned path when the path is near a obstacle - occ_path_level: 120.0 # Threshold level for a obstacle - - cte_look_ahead: 0.2 # The max/min distance to add for the CTE tracking - - penalize_negative_x: true # Whether to penalize trajectories that have negative x velocities. - - # non-dynamic parameters - dist_towards_obstacles: 1.5 - dist_towards_obstacles_trolley: 1.75 - goal_seek_tolerance: 2.0 - goal_seek_tolerance_trolley: 0.25 diff --git a/mir_navigation/config/mir_mapping_async.yaml b/mir_navigation/config/mir_mapping_async.yaml new file mode 100644 index 00000000..332aeec9 --- /dev/null +++ b/mir_navigation/config/mir_mapping_async.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + # map_file_name: #install/share/mir_navigation/maps/maze_incomplete + # map_start_pose: [0.0, 0.0, 0.0] + # map_start_at_dock: true + + debug_logging: true + throttle_scans: 1 + transform_publish_period: 0.05 # if 0 never publishes odometry + map_update_interval: 0.01 + resolution: 0.05 + max_laser_range: 10.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.5 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 20 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/mir_navigation/config/mir_mapping_async_namespaced.yaml b/mir_navigation/config/mir_mapping_async_namespaced.yaml new file mode 100644 index 00000000..93b0ebb9 --- /dev/null +++ b/mir_navigation/config/mir_mapping_async_namespaced.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: namespace/odom + map_frame: map + base_frame: namespace/base_footprint + scan_topic: /namespace/scan + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + # map_file_name: #install/share/mir_navigation/maps/maze_incomplete + # map_start_pose: [0.0, 0.0, 0.0] + # map_start_at_dock: true + + debug_logging: true + throttle_scans: 1 + transform_publish_period: 0.05 # if 0 never publishes odometry + map_update_interval: 0.01 + resolution: 0.05 + max_laser_range: 10.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.5 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 20 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/mir_navigation/config/mir_mapping_async_sim.yaml b/mir_navigation/config/mir_mapping_async_sim.yaml new file mode 100644 index 00000000..158520c4 --- /dev/null +++ b/mir_navigation/config/mir_mapping_async_sim.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + # map_file_name: #install/share/mir_navigation/maps/maze_incomplete + # map_start_pose: [0.0, 0.0, 0.0] + # map_start_at_dock: true + + debug_logging: true + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 0.01 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.5 + tf_buffer_duration: 300. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/mir_navigation/config/mir_mapping_async_sim_namespaced.yaml b/mir_navigation/config/mir_mapping_async_sim_namespaced.yaml new file mode 100644 index 00000000..449a2299 --- /dev/null +++ b/mir_navigation/config/mir_mapping_async_sim_namespaced.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: namespace/odom + map_frame: map + base_frame: namespace/base_footprint + scan_topic: /namespace/scan + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + # map_file_name: #install/share/mir_navigation/maps/maze_incomplete + # map_start_pose: [0.0, 0.0, 0.0] + # map_start_at_dock: true + + debug_logging: true + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 0.01 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.5 + tf_buffer_duration: 300. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/mir_navigation/config/mir_nav_params.yaml b/mir_navigation/config/mir_nav_params.yaml new file mode 100644 index 00000000..ee858885 --- /dev/null +++ b/mir_navigation/config/mir_nav_params.yaml @@ -0,0 +1,338 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 2.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + # + # Note: Not quite correct, params that are not mentioned can not be substituted! + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.1 + # speed_limit_topic : "speed_limit" + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.15 + stateful: True + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 2.5 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 2.5 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist", "ObstacleFootprint"] + BaseObstacle.scale: 0.02 + GoalAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.325 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 24.0 + RotateToGoal.xy_goal_tolerance: 0.25 + RotateToGoal.slowing_factor: 2.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + # offset: front / back from base_link + # footprint: '[ [front, width/2], [back, width/2], [-back, -width/2], [front, -width/2] ]' + footprint: '[ [0.5, 0.25], [-0.4, 0.25], [-0.4, -0.25], [0.5, -0.25] ]' + footprint_padding: 0.01 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + inflation_radius: 0.55 + cost_scaling_factor: 3.0 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + expected_update_rate: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + # offset: front / back from base_link + # footprint: '[ [front, width/2], [back, width/2], [-back, -width/2], [front, -width/2] ]' + footprint: '[ [0.5, 0.25], [-0.4, 0.25], [-0.4, -0.25], [0.5, -0.25] ]' + footprint_padding: 0.01 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + footprint_clearing_enabled: true + max_obstacle_height: 2.0 + combination_method: 1 + scan: + topic: /scan + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + max_obstacle_height: 2.0 + min_obstacle_height: 0.0 + clearing: True + marking: True + data_type: "LaserScan" + inf_is_valid: false + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: true + inflation_radius: 0.55 + cost_scaling_factor: 3.0 + inflate_unknown: false + inflate_around_unknown: true + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: true + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 0.5 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/mir_navigation/config/mir_nav_params_namespaced.yaml b/mir_navigation/config/mir_nav_params_namespaced.yaml new file mode 100644 index 00000000..446d1fea --- /dev/null +++ b/mir_navigation/config/mir_nav_params_namespaced.yaml @@ -0,0 +1,337 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "namespace/base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "namespace/odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "differential" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 2.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: /namespace/scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: namespace/base_link + odom_topic: /namespace/odom + bt_loop_duration: 10 + default_server_timeout: 20 + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + # + # Note: Not quite correct, params that are not mentioned can not be substituted! + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.1 + # speed_limit_topic : "speed_limit" + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.15 + stateful: True + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 2.5 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 2.5 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist", "ObstacleFootprint"] + BaseObstacle.scale: 0.02 + GoalAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.325 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 24.0 + RotateToGoal.xy_goal_tolerance: 0.25 + RotateToGoal.slowing_factor: 2.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: namespace/odom + robot_base_frame: namespace/base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + # offset: front / back from base_link + # footprint: '[ [front, width/2], [back, width/2], [-back, -width/2], [front, -width/2] ]' + footprint: '[ [0.5, 0.25], [-0.4, 0.25], [-0.4, -0.25], [0.5, -0.25] ]' + footprint_padding: 0.01 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + inflation_radius: 0.55 + cost_scaling_factor: 3.0 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /namespace/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + expected_update_rate: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: namespace/base_link + use_sim_time: True + # offset: front / back from base_link + # footprint: '[ [front, width/2], [back, width/2], [-back, -width/2], [front, -width/2] ]' + footprint: '[ [0.5, 0.25], [-0.4, 0.25], [-0.4, -0.25], [0.5, -0.25] ]' + footprint_padding: 0.01 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + footprint_clearing_enabled: true + max_obstacle_height: 2.0 + combination_method: 1 + scan: + topic: /namespace/scan + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + max_obstacle_height: 2.0 + min_obstacle_height: 0.0 + clearing: True + marking: True + data_type: "LaserScan" + inf_is_valid: false + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: true + inflation_radius: 0.55 + cost_scaling_factor: 3.0 + inflate_unknown: false + inflate_around_unknown: true + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: true + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: namespace/odom + robot_base_frame: namespace/base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 0.5 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/mir_navigation/config/move_base_common_params.yaml b/mir_navigation/config/move_base_common_params.yaml deleted file mode 100644 index 9d269792..00000000 --- a/mir_navigation/config/move_base_common_params.yaml +++ /dev/null @@ -1,17 +0,0 @@ -### footprint -footprint: [[0.506,-0.32],[0.506,0.32],[-0.454,0.32],[-0.454,-0.32]] -footprint_padding: 0.0 - -### replanning -controller_frequency: 5.0 # run controller at 5.0 Hz -controller_patience: 15.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan -planner_frequency: 0.0 # don't continually replan (only when controller failed) -planner_patience: 5.0 # if the first planning attempt failed, abort planning retries after 5.0 s... -max_planning_retries: 10 # ... or after 10 attempts (whichever happens first) -oscillation_timeout: 30.0 # abort controller and trigger recovery behaviors after 30.0 s - -### recovery behaviors -recovery_behavior_enabled: true -recovery_behaviors: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}] -conservative_reset: - reset_distance: 3.0 # clear obstacles farther away than 3.0 m diff --git a/mir_navigation/config/pose_local_planner_params.yaml b/mir_navigation/config/pose_local_planner_params.yaml deleted file mode 100644 index 238e1133..00000000 --- a/mir_navigation/config/pose_local_planner_params.yaml +++ /dev/null @@ -1,46 +0,0 @@ -base_local_planner: pose_follower/PoseFollower -PoseFollower: - k_trans: 2.0 - k_rot: 1.0 - - # within this distance to the goal, finally rotate to the goal heading (also, we've reached our goal only if we're within this dist) - tolerance_trans: 0.3 - - # we've reached our goal only if we're within this angular distance - tolerance_rot: 0.3 - - # we've reached our goal only if we're within range for this long and stopped - tolerance_timeout: 0.5 - - # set this to true if you're using a holonomic robot - holonomic: false - - # number of samples (scaling factors of our current desired twist) to check the validity of - samples: 10 - - # go no faster than this - max_vel_lin: 0.3 - max_vel_th: 0.5 - - # minimum velocities to keep from getting stuck - min_vel_lin: 0.03 - min_vel_th: 0.1 - - # if we're rotating in place, go at least this fast to avoid getting stuck - min_in_place_vel_th: 0.1 - - # when we're near the end and would be trying to go no faster than this translationally, just rotate in place instead - in_place_trans_vel: 0.1 - - # we're "stopped" if we're going slower than these velocities - trans_stopped_velocity: 0.03 - rot_stopped_velocity: 0.1 - - # if this is true, we don't care whether we go backwards or forwards - allow_backwards: true - - # if this is true, turn in place to face the new goal instead of arcing toward it - turn_in_place_first: true - - # if turn_in_place_first is true, turn in place if our heading is more than this far from facing the goal location - max_heading_diff_before_moving: 0.2 diff --git a/mir_navigation/config/sbpl_global_params.yaml b/mir_navigation/config/sbpl_global_params.yaml deleted file mode 100644 index 8fdc0b47..00000000 --- a/mir_navigation/config/sbpl_global_params.yaml +++ /dev/null @@ -1,9 +0,0 @@ -base_global_planner: SBPLLatticePlanner -SBPLLatticePlanner: - environment_type: XYThetaLattice - planner_type: ARAPlanner - allocated_time: 10.0 - initial_epsilon: 15.0 - forward_search: false - nominalvel_mpersecs: 0.8 - timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s diff --git a/mir_navigation/config/teb_local_planner_params.yaml b/mir_navigation/config/teb_local_planner_params.yaml deleted file mode 100644 index 5e395af6..00000000 --- a/mir_navigation/config/teb_local_planner_params.yaml +++ /dev/null @@ -1,105 +0,0 @@ -# NOTE: When using the teb_local_planner, make sure to set the local costmap -# resolution high (for example 0.1 m), otherwise the optimization will take -# forever (around 10 minutes for each iteration). -base_local_planner: teb_local_planner/TebLocalPlannerROS -TebLocalPlannerROS: - # Trajectory - teb_autosize: true # Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended) - dt_ref: 0.3 # Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate) - dt_hysteresis: 0.1 # Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref - global_plan_overwrite_orientation: false # Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically - allow_init_with_backwards_motion: false # If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors) - max_global_plan_lookahead_dist: 3.0 # Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size] - force_reinit_new_goal_dist: 1.0 # Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting) - feasibility_check_no_poses: 5 # Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval - global_plan_viapoint_sep: -0.1 # Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled] - via_points_ordered: false # If true, the planner adheres to the order of via-points in the storage container - exact_arc_length: false # If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used. - publish_feedback: false # Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes) - - # Robot - max_vel_x: 0.8 # Maximum translational velocity of the robot - max_vel_x_backwards: 0.2 # Maximum translational velocity of the robot for driving backwards - max_vel_y: 0.0 # Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) - max_vel_theta: 1.0 # Maximum angular velocity of the robot - acc_lim_x: 1.5 # Maximum translational acceleration of the robot - acc_lim_y: 0.0 # Maximum strafing acceleration of the robot - acc_lim_theta: 2.0 # Maximum angular acceleration of the robot - min_turning_radius: 0.0 # Minimum turning radius of a carlike robot (diff-drive robot: zero) - wheelbase: 1.0 # The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots! - cmd_angle_instead_rotvel: false # Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance') - is_footprint_dynamic: false # If true, update the footprint before checking trajectory feasibility - footprint_model: - type: "polygon" - vertices: [[0.506,-0.32],[0.506,0.32],[-0.454,0.32],[-0.454,-0.32]] - - # Goal tolerance - xy_goal_tolerance: 0.03 # Allowed final euclidean distance to the goal position - yaw_goal_tolerance: 0.08 # Allowed final orientation error to the goal orientation - free_goal_vel: false # Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed) - - # Obstacles - min_obstacle_dist: 0.4 # Minimum desired separation from obstacles - inflation_dist: 0.1 # Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect) - dynamic_obstacle_inflation_dist: 0.6 # Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect) - include_dynamic_obstacles: false # Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static. - include_costmap_obstacles: true # Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented) - legacy_obstacle_association: false # If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles). - obstacle_association_force_inclusion_factor: 1.5 # The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist. - obstacle_association_cutoff_factor: 5.0 # See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first. - costmap_obstacles_behind_robot_dist: 1.5 # Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters) - obstacle_poses_affected: 10 # The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well - #costmap_converter_plugin: "" # - #costmap_converter_spin_thread: true # - #costmap_converter_rate: 5 # - - # Optimization - no_inner_iterations: 5 # Number of solver iterations called in each outerloop iteration - no_outer_iterations: 4 # Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations - optimization_activate: true # Activate the optimization - optimization_verbose: false # Print verbose information - penalty_epsilon: 0.1 # Add a small safty margin to penalty functions for hard-constraint approximations - weight_max_vel_x: 2.0 # Optimization weight for satisfying the maximum allowed translational velocity - weight_max_vel_y: 2.0 # Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots) - weight_max_vel_theta: 1.0 # Optimization weight for satisfying the maximum allowed angular velocity - weight_acc_lim_x: 1.0 # Optimization weight for satisfying the maximum allowed translational acceleration - weight_acc_lim_y: 1.0 # Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots) - weight_acc_lim_theta: 1.0 # Optimization weight for satisfying the maximum allowed angular acceleration - weight_kinematics_nh: 1000.0 # Optimization weight for satisfying the non-holonomic kinematics - weight_kinematics_forward_drive: 1.0 # Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot) - weight_kinematics_turning_radius: 1.0 # Optimization weight for enforcing a minimum turning radius (carlike robots) - weight_optimaltime: 1.0 # Optimization weight for contracting the trajectory w.r.t transition time - weight_obstacle: 50.0 # Optimization weight for satisfying a minimum seperation from obstacles - weight_inflation: 0.1 # Optimization weight for the inflation penalty (should be small) - weight_dynamic_obstacle: 50.0 # Optimization weight for satisfying a minimum seperation from dynamic obstacles - weight_dynamic_obstacle_inflation: 0.1 # Optimization weight for the inflation penalty of dynamic obstacles (should be small) - weight_viapoint: 1.0 # Optimization weight for minimizing the distance to via-points - weight_adapt_factor: 2.0 # Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem. - - # Homotopy Class Planner -# enable_homotopy_class_planning: true # - enable_multithreading: true # Activate multiple threading for planning multiple trajectories in parallel -# simple_exploration: false # - max_number_classes: 2 # Specify the maximum number of allowed alternative homotopy classes (limits computational effort) - selection_cost_hysteresis: 1.0 # Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor) - selection_prefer_initial_plan: 0.95 # Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.) - selection_obst_cost_scale: 100.0 # Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor) - selection_viapoint_cost_scale: 1.0 # Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor) - selection_alternative_time_cost: false # If true, time cost is replaced by the total transition time. - roadmap_graph_no_samples: 15 # Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off - roadmap_graph_area_width: 5.0 # Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance) - roadmap_graph_area_length_scale: 1.0 # The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!) - h_signature_prescaler: 1.0 # Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mir_navigation/launch/hector_mapping.launch b/mir_navigation/launch/hector_mapping.launch deleted file mode 100644 index ea5c4b8d..00000000 --- a/mir_navigation/launch/hector_mapping.launch +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mir_navigation/launch/include/amcl.py b/mir_navigation/launch/include/amcl.py new file mode 100644 index 00000000..56681778 --- /dev/null +++ b/mir_navigation/launch/include/amcl.py @@ -0,0 +1,103 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + mir_nav_dir = get_package_share_directory('mir_navigation') + + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + lifecycle_nodes = ['map_server', 'amcl'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + return LaunchDescription([ + # Set env var to print messages to stdout immediately + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + + DeclareLaunchArgument( + 'map', + default_value=os.path.join(mir_nav_dir, 'maps', 'maze.yaml'), + description='Full path to map yaml file to load'), + + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack'), + + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join( + mir_nav_dir, 'config', 'mir_nav_params.yaml'), + description='Full path to the ROS2 parameters file to use'), + + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_amcl', + executable='amcl', + name='amcl', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_localization', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ]) diff --git a/mir_navigation/launch/include/mapping.py b/mir_navigation/launch/include/mapping.py new file mode 100644 index 00000000..b9d3c19c --- /dev/null +++ b/mir_navigation/launch/include/mapping.py @@ -0,0 +1,34 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + ld = LaunchDescription() + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation/Gazebo clock') + + declare_slam_params_file_cmd = DeclareLaunchArgument( + 'slam_params_file', + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + launch_mapping = Node( + parameters=[ + LaunchConfiguration('slam_params_file'), + {'use_sim_time': LaunchConfiguration('use_sim_time')} + ], + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen') + + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(launch_mapping) + + return ld diff --git a/mir_navigation/launch/include/navigation.py b/mir_navigation/launch/include/navigation.py new file mode 100644 index 00000000..e9933280 --- /dev/null +++ b/mir_navigation/launch/include/navigation.py @@ -0,0 +1,173 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, OpaqueFunction, \ + SetLaunchConfiguration +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + mir_nav_dir = get_package_share_directory('mir_navigation') + + use_sim_time = LaunchConfiguration('use_sim_time') + command_topic = LaunchConfiguration('cmd_vel_w_prefix') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + default_nav_to_pose_bt_xml = LaunchConfiguration( + 'default_nav_to_pose_bt_xml') + map_subscribe_transient_local = LaunchConfiguration( + 'map_subscribe_transient_local') + + lifecycle_nodes = ['controller_server', + 'planner_server', + 'recoveries_server', + 'bt_navigator', + 'waypoint_follower'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('cmd_vel', command_topic)] + + # Create our own temporary YAML files that include substitutions + # Watch out for parameters that don't exist in yaml - will not be substituted of course + # (default_nav_to_pose_bt_xml) + # TODO: Needs to be addressed in nav2 + param_substitutions = { + 'use_sim_time': use_sim_time, + 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, + 'autostart': autostart, + 'map_subscribe_transient_local': map_subscribe_transient_local} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + def add_prefix_to_cmd_vel(context): + topic = context.launch_configurations['cmd_vel_topic'] + try: + namespace = context.launch_configurations['namespace'] + topic = namespace + '/' + topic + except KeyError: + pass + return [SetLaunchConfiguration('cmd_vel_w_prefix', topic)] + + return LaunchDescription([ + # Set env var to print messages to stdout immediately + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + + DeclareLaunchArgument( + 'namespace', default_value='', + description='Top-level namespace'), + + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'cmd_vel_topic', default_value='cmd_vel', + description='Define cmd_vel topic'), + + DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack'), + + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join( + mir_nav_dir, 'config', 'mir_nav_params.yaml'), + description='Full path to the ROS2 parameters file to use'), + + DeclareLaunchArgument( + 'default_nav_to_pose_bt_xml', + default_value=os.path.join( + mir_nav_dir, 'behavior_trees', 'navigate_to_pose_w_replanning_and_recovery.xml'), + description='Full path to the behavior tree xml file to use'), + + DeclareLaunchArgument( + 'map_subscribe_transient_local', default_value='true', + description='Whether to set the map subscriber QoS to transient local'), + + OpaqueFunction(function=add_prefix_to_cmd_vel), + + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + parameters=[configured_params], + remappings=remappings + ), + + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[configured_params], + remappings=remappings + ), + + Node( + package='nav2_recoveries', + executable='recoveries_server', + name='recoveries_server', + output='screen', + parameters=[configured_params], + remappings=remappings + ), + + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + parameters=[ + configured_params, + {'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml}], + remappings=remappings + ), + + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + parameters=[configured_params], + remappings=remappings + ), + + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ]) diff --git a/mir_navigation/launch/mir_mapping_launch.py b/mir_navigation/launch/mir_mapping_launch.py new file mode 100644 index 00000000..24e2ac0e --- /dev/null +++ b/mir_navigation/launch/mir_mapping_launch.py @@ -0,0 +1,81 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, \ + SetLaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + mir_driver_dir = get_package_share_directory('mir_driver') + mir_nav_dir = get_package_share_directory('mir_navigation') + + def declare_rviz_config(context): + nav_enabled = context.launch_configurations['navigation_enabled'] + if (nav_enabled == 'true'): + config_file = os.path.join( + mir_nav_dir, 'rviz', 'mir_mapping_nav.rviz') + else: + config_file = os.path.join(mir_nav_dir, 'rviz', 'mir_mapping.rviz') + return [SetLaunchConfiguration('rviz_config_file', config_file)] + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation/Gazebo clock') + + declare_slam_params_file_cmd = DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(get_package_share_directory("mir_navigation"), + 'config', 'mir_mapping_async.yaml'), + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + declare_nav_argument = DeclareLaunchArgument( + 'navigation_enabled', + default_value='false', + description='Use navigation for mapping') + + declare_namespace_arg = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Namespace to push all topics into.') + + start_driver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_driver_dir, 'launch', 'mir_launch.py')), + launch_arguments={'rviz_config_file': LaunchConfiguration( + 'rviz_config_file')}.items() + ) + + launch_mapping = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_nav_dir, 'launch', 'include', 'mapping.py')), + launch_arguments=[ + ('use_sim_time', LaunchConfiguration('use_sim_time')), + ('slam_params_file', LaunchConfiguration('slam_params_file'))] + ) + + launch_navigation_if_enabled = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_nav_dir, 'launch', 'include', 'navigation.py')), + condition=IfCondition(LaunchConfiguration('navigation_enabled')), + launch_arguments={'map_subscribe_transient_local': 'true'}.items() + ) + + ld = LaunchDescription() + + ld.add_action(declare_namespace_arg) + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_nav_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(OpaqueFunction(function=declare_rviz_config)) + + ld.add_action(start_driver_cmd) + ld.add_action(launch_mapping) + ld.add_action(launch_navigation_if_enabled) + + return ld diff --git a/mir_navigation/launch/mir_mapping_sim_launch.py b/mir_navigation/launch/mir_mapping_sim_launch.py new file mode 100644 index 00000000..1d7bdb99 --- /dev/null +++ b/mir_navigation/launch/mir_mapping_sim_launch.py @@ -0,0 +1,88 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, \ + SetLaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + mir_gazebo_dir = get_package_share_directory('mir_gazebo') + mir_nav_dir = get_package_share_directory('mir_navigation') + + def declare_rviz_config(context): + nav_enabled = context.launch_configurations['navigation_enabled'] + if (nav_enabled == 'true'): + config_file = os.path.join( + mir_nav_dir, 'rviz', 'mir_mapping_nav.rviz') + else: + config_file = os.path.join(mir_nav_dir, 'rviz', 'mir_mapping.rviz') + return [SetLaunchConfiguration('rviz_config_file', config_file)] + + declare_namespace_arg = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Namespace to push all topics into.') + + declare_world_argument = DeclareLaunchArgument( + 'world', + default_value='maze', + description='Choose simulation world. Available worlds: empty, maze') + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation/Gazebo clock') + + declare_slam_params_file_cmd = DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(get_package_share_directory("mir_navigation"), + 'config', 'mir_mapping_async_sim.yaml'), + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + declare_nav_argument = DeclareLaunchArgument( + 'navigation_enabled', + default_value='false', + description='Use navigation for mapping') + + launch_simulation = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_gazebo_dir, 'launch', 'mir_gazebo_launch.py')), + launch_arguments={'rviz_config_file': LaunchConfiguration( + 'rviz_config_file')}.items() + ) + + launch_mapping = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_nav_dir, 'launch', 'include', 'mapping.py')), + launch_arguments=[ + ('use_sim_time', LaunchConfiguration('use_sim_time')), + ('slam_params_file', LaunchConfiguration('slam_params_file'))] + ) + + launch_navigation_if_enabled = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_nav_dir, 'launch', 'include', 'navigation.py')), + condition=IfCondition(LaunchConfiguration('navigation_enabled')), + launch_arguments=[ + ('use_sim_time', LaunchConfiguration('use_sim_time'))] + ) + + ld = LaunchDescription() + + ld.add_action(declare_namespace_arg) + ld.add_action(declare_world_argument) + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_nav_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(OpaqueFunction(function=declare_rviz_config)) + + ld.add_action(launch_simulation) + ld.add_action(launch_mapping) + ld.add_action(launch_navigation_if_enabled) + + return ld diff --git a/mir_navigation/launch/mir_nav_launch.py b/mir_navigation/launch/mir_nav_launch.py new file mode 100644 index 00000000..b247c5e4 --- /dev/null +++ b/mir_navigation/launch/mir_nav_launch.py @@ -0,0 +1,74 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, \ + SetLaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + mir_driver_dir = get_package_share_directory('mir_driver') + mir_nav_dir = get_package_share_directory('mir_navigation') + + def find_map_file(context): + map_arg = context.launch_configurations['map'] + if(os.path.isfile(os.path.join(mir_nav_dir, 'maps', map_arg))): + return[SetLaunchConfiguration('map_file', os.path.join(mir_nav_dir, 'maps', map_arg))] + elif (os.path.isfile(map_arg)): + return[SetLaunchConfiguration('map_file', map_arg)] + + declare_map_file_argument = DeclareLaunchArgument( + 'map', + description='Relative path to map in mir_navigation/maps or full path to map (yaml).') + + declare_rviz_config_file_argument = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join(mir_nav_dir, 'rviz', 'mir_nav.rviz'), + description='Full path to rviz configuration file') + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation/Gazebo clock') + + declare_slam_params_file_cmd = DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(get_package_share_directory("mir_navigation"), + 'config', 'mir_mapping_async.yaml'), + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + start_driver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_driver_dir, 'launch', 'mir_launch.py')), + launch_arguments={'rviz_config_file': LaunchConfiguration( + 'rviz_config_file')}.items() + ) + + launch_amcl = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_nav_dir, 'launch', 'include', 'amcl.py')), + launch_arguments={'map': LaunchConfiguration('map_file')}.items() + ) + + launch_navigation = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_nav_dir, 'launch', 'include', 'navigation.py')), + launch_arguments={'map_subscribe_transient_local': 'true'}.items() + ) + + ld = LaunchDescription() + + ld.add_action(declare_map_file_argument) + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(declare_rviz_config_file_argument) + ld.add_action(OpaqueFunction(function=find_map_file)) + + ld.add_action(start_driver_cmd) + ld.add_action(launch_amcl) + ld.add_action(launch_navigation) + + return ld diff --git a/mir_navigation/launch/mir_nav_sim_launch.py b/mir_navigation/launch/mir_nav_sim_launch.py new file mode 100644 index 00000000..36175e7d --- /dev/null +++ b/mir_navigation/launch/mir_nav_sim_launch.py @@ -0,0 +1,87 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, \ + SetLaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + mir_gazebo_dir = get_package_share_directory('mir_gazebo') + mir_nav_dir = get_package_share_directory('mir_navigation') + + def find_map_file(context): + map_arg = context.launch_configurations['map'] + if(os.path.isfile(os.path.join(mir_nav_dir, 'maps', map_arg))): + return[SetLaunchConfiguration('map_file', os.path.join(mir_nav_dir, 'maps', map_arg))] + elif (os.path.isfile(map_arg)): + return[SetLaunchConfiguration('map_file', map_arg)] + + declare_namespace_arg = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Namespace to push all topics into.') + + declare_world_argument = DeclareLaunchArgument( + 'world', + default_value='maze', + description='Choose simulation world. Available worlds: empty, maze') + + declare_map_file_argument = DeclareLaunchArgument( + 'map', + default_value=os.path.join(mir_nav_dir, 'maps', 'maze.yaml'), + description='Relative path to map in mir_navigation/maps or full path to map (yaml).') + + declare_rviz_config_file_argument = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join(mir_nav_dir, 'rviz', 'mir_nav.rviz'), + description='Full path to rviz configuration file') + + declare_use_sim_time_argument = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation/Gazebo clock') + + declare_slam_params_file_cmd = DeclareLaunchArgument( + 'slam_params_file', + default_value=os.path.join(get_package_share_directory("mir_navigation"), + 'config', 'mir_mapping_async_sim.yaml'), + description='Full path to the ROS2 parameters file to use for the slam_toolbox node') + + launch_simulation = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_gazebo_dir, 'launch', 'mir_gazebo_launch.py')), + launch_arguments={'rviz_config_file': LaunchConfiguration( + 'rviz_config_file')}.items() + ) + + launch_amcl = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_nav_dir, 'launch', 'include', 'amcl.py')), + launch_arguments={'map': LaunchConfiguration('map_file')}.items() + ) + + launch_navigation = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(mir_nav_dir, 'launch', 'include', 'navigation.py')), + launch_arguments={'map_subscribe_transient_local': 'true'}.items() + ) + + ld = LaunchDescription() + + ld.add_action(declare_namespace_arg) + ld.add_action(declare_world_argument) + ld.add_action(declare_map_file_argument) + ld.add_action(declare_use_sim_time_argument) + ld.add_action(declare_slam_params_file_cmd) + ld.add_action(declare_rviz_config_file_argument) + ld.add_action(OpaqueFunction(function=find_map_file)) + + ld.add_action(launch_simulation) + ld.add_action(launch_amcl) + ld.add_action(launch_navigation) + + return ld diff --git a/mir_navigation/launch/move_base.xml b/mir_navigation/launch/move_base.xml deleted file mode 100644 index 090fe7ef..00000000 --- a/mir_navigation/launch/move_base.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mir_navigation/launch/start_maps.launch b/mir_navigation/launch/start_maps.launch deleted file mode 100644 index 7e699e40..00000000 --- a/mir_navigation/launch/start_maps.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/mir_navigation/launch/start_planner.launch b/mir_navigation/launch/start_planner.launch deleted file mode 100644 index 8bbf6980..00000000 --- a/mir_navigation/launch/start_planner.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mir_navigation/maps/maze.data b/mir_navigation/maps/maze.data new file mode 100644 index 00000000..2cf4fd00 Binary files /dev/null and b/mir_navigation/maps/maze.data differ diff --git a/mir_navigation/maps/maze.pgm b/mir_navigation/maps/maze.pgm new file mode 100644 index 00000000..a79cb083 Binary files /dev/null and b/mir_navigation/maps/maze.pgm differ diff --git a/mir_navigation/maps/maze.posegraph b/mir_navigation/maps/maze.posegraph new file mode 100644 index 00000000..20277142 Binary files /dev/null and b/mir_navigation/maps/maze.posegraph differ diff --git a/mir_navigation/maps/maze.yaml b/mir_navigation/maps/maze.yaml new file mode 100644 index 00000000..fdf9de62 --- /dev/null +++ b/mir_navigation/maps/maze.yaml @@ -0,0 +1,7 @@ +image: maze.pgm +mode: trinary +resolution: 0.05 +origin: [-9.96, -9.8, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/mir_navigation/mprim/genmprim_unicycle_highcost_5cm.m b/mir_navigation/mprim/genmprim_unicycle_highcost_5cm.m deleted file mode 100644 index 28105933..00000000 --- a/mir_navigation/mprim/genmprim_unicycle_highcost_5cm.m +++ /dev/null @@ -1,280 +0,0 @@ -% /* -% * Copyright (c) 2008, Maxim Likhachev -% * All rights reserved. -% * -% * Redistribution and use in source and binary forms, with or without -% * modification, are permitted provided that the following conditions are met: -% * -% * * Redistributions of source code must retain the above copyright -% * notice, this list of conditions and the following disclaimer. -% * * Redistributions in binary form must reproduce the above copyright -% * notice, this list of conditions and the following disclaimer in the -% * documentation and/or other materials provided with the distribution. -% * * Neither the name of the Carnegie Mellon University nor the names of its -% * contributors may be used to endorse or promote products derived from -% * this software without specific prior written permission. -% * -% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -% * POSSIBILITY OF SUCH DAMAGE. -% */ - -function[] = genmprim_unicycle_highcost_5cm(outfilename) - -% -%generates motion primitives and saves them into file -% -%written by Maxim Likhachev -%--------------------------------------------------- -% - -%defines - -UNICYCLE_MPRIM_16DEGS = 1; - - -if UNICYCLE_MPRIM_16DEGS == 1 - resolution = 0.05; - numberofangles = 16; %preferably a power of 2, definitely multiple of 8 - numberofprimsperangle = 7; - - %multipliers (multiplier is used as costmult*cost) - forwardcostmult = 1; - backwardcostmult = 40; - forwardandturncostmult = 2; - sidestepcostmult = 10; - turninplacecostmult = 20; - - %note, what is shown x,y,theta changes (not absolute numbers) - - %0 degreees - basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult - %x aligned with the heading of the robot, angles are positive - %counterclockwise - %0 theta change - basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult]; - basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult]; - basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult]; - %1/16 theta change - basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult]; - basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult]; - %turn in place - basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult]; - basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult]; - - %45 degrees - basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost) - %x aligned with the heading of the robot, angles are positive - %counterclockwise - %0 theta change - basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult]; - basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult]; - basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult]; - %1/16 theta change - basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult]; - basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult]; - %turn in place - basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult]; - basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult]; - - %22.5 degrees - basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost) - %x aligned with the heading of the robot, angles are positive - %counterclockwise - %0 theta change - basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult]; - basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult]; - basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult]; - %1/16 theta change - basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult]; - basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult]; - %turn in place - basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult]; - basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult]; - -else - fprintf(1, 'ERROR: undefined mprims type\n'); - return; -end; - - -fout = fopen(outfilename, 'w'); - - -%write the header -fprintf(fout, 'resolution_m: %f\n', resolution); -fprintf(fout, 'numberofangles: %d\n', numberofangles); -fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles); - -%iterate over angles -for angleind = 1:numberofangles - - figure(1); - hold off; - - text(0, 0, int2str(angleind)); - - %iterate over primitives - for primind = 1:numberofprimsperangle - fprintf(fout, 'primID: %d\n', primind-1); - fprintf(fout, 'startangle_c: %d\n', angleind-1); - - %current angle - currentangle = (angleind-1)*2*pi/numberofangles; - currentangle_36000int = round((angleind-1)*36000/numberofangles); - - %compute which template to use - if (rem(currentangle_36000int, 9000) == 0) - basemprimendpts_c = basemprimendpts0_c(primind,:); - angle = currentangle; - elseif (rem(currentangle_36000int, 4500) == 0) - basemprimendpts_c = basemprimendpts45_c(primind,:); - angle = currentangle - 45*pi/180; - elseif (rem(currentangle_36000int-7875, 9000) == 0) - basemprimendpts_c = basemprimendpts33p75_c(primind,:); - basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y - basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1); - basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well - angle = currentangle - 78.75*pi/180; - fprintf(1, '78p75\n'); - elseif (rem(currentangle_36000int-6750, 9000) == 0) - basemprimendpts_c = basemprimendpts22p5_c(primind,:); - basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y - basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1); - basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well - %fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ... - % basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3)); - angle = currentangle - 67.5*pi/180; - fprintf(1, '67p5\n'); - elseif (rem(currentangle_36000int-5625, 9000) == 0) - basemprimendpts_c = basemprimendpts11p25_c(primind,:); - basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y - basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1); - basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well - angle = currentangle - 56.25*pi/180; - fprintf(1, '56p25\n'); - elseif (rem(currentangle_36000int-3375, 9000) == 0) - basemprimendpts_c = basemprimendpts33p75_c(primind,:); - angle = currentangle - 33.75*pi/180; - fprintf(1, '33p75\n'); - elseif (rem(currentangle_36000int-2250, 9000) == 0) - basemprimendpts_c = basemprimendpts22p5_c(primind,:); - angle = currentangle - 22.5*pi/180; - fprintf(1, '22p5\n'); - elseif (rem(currentangle_36000int-1125, 9000) == 0) - basemprimendpts_c = basemprimendpts11p25_c(primind,:); - angle = currentangle - 11.25*pi/180; - fprintf(1, '11p25\n'); - else - fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int); - return; - end; - - %now figure out what action will be - baseendpose_c = basemprimendpts_c(1:3); - additionalactioncostmult = basemprimendpts_c(4); - endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle)); - endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle)); - endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles); - endpose_c = [endx_c endy_c endtheta_c]; - - fprintf(1, 'rotation angle=%f\n', angle*180/pi); - - if baseendpose_c(2) == 0 & baseendpose_c(3) == 0 - %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3)); - end; - - %generate intermediate poses (remember they are w.r.t 0,0 (and not - %centers of the cells) - numofsamples = 10; - intermcells_m = zeros(numofsamples,3); - if UNICYCLE_MPRIM_16DEGS == 1 - startpt = [0 0 currentangle]; - endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ... - rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles]; - intermcells_m = zeros(numofsamples,3); - if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward - for iind = 1:numofsamples - intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ... - startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ... - 0]; - rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles); - intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi); - - end; - else %unicycle-based move forward or backward - R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3)); - sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))]; - S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)]; - l = S(1); - tvoverrv = S(2); - rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv); - tv = tvoverrv*rv; - - if l < 0 - fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l); - l = 0; - end; - %compute rv - %rv = baseendpose_c(3)*2*pi/numberofangles; - %compute tv - %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3))) - %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3))) - %tv = (tvx + tvy)/2.0; - %generate samples - for iind = 1:numofsamples - dt = (iind-1)/(numofsamples-1); - - %dtheta = rv*dt + startpt(3); - %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ... - % startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ... - % dtheta]; - - if(dt*tv < l) - intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ... - startpt(2) + dt*tv*sin(startpt(3)) ... - startpt(3)]; - else - dtheta = rv*(dt - l/tv) + startpt(3); - intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ... - startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ... - dtheta]; - end; - end; - %correct - errorxy = [endpt(1) - intermcells_m(numofsamples,1) ... - endpt(2) - intermcells_m(numofsamples,2)]; - fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2)); - interpfactor = [0:1/(numofsamples-1):1]; - intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor'; - intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor'; - end; - end; - - %write out - fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3)); - fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult); - fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1)); - for interind = 1:size(intermcells_m, 1) - fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3)); - end; - - plot(intermcells_m(:,1), intermcells_m(:,2)); - axis([-0.3 0.3 -0.3 0.3]); - text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3))); - hold on; - - end; - grid; - pause; -end; - -fclose('all'); diff --git a/mir_navigation/mprim/genmprim_unicycle_highcost_5cm.py b/mir_navigation/mprim/genmprim_unicycle_highcost_5cm.py deleted file mode 100755 index bf694be9..00000000 --- a/mir_navigation/mprim/genmprim_unicycle_highcost_5cm.py +++ /dev/null @@ -1,392 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2016, David Conner (Christopher Newport University) -# Based on genmprim_unicycle.m -# Copyright (c) 2008, Maxim Likhachev -# All rights reserved. -# converted by libermate utility (https://github.com/awesomebytes/libermate) -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Carnegie Mellon University nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import numpy as np -import rospkg - -# if available import pylab (from matlibplot) -matplotlib_found = False -try: - import matplotlib.pylab as plt - - matplotlib_found = True -except ImportError: - pass - - -def matrix_size(mat, elem=None): - if not elem: - return mat.shape - else: - return mat.shape[int(elem) - 1] - - -def genmprim_unicycle(outfilename, visualize=False, separate_plots=False): - visualize = matplotlib_found and visualize # Plot the primitives - - # Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c, baseendpose_c, additionalactioncostmult, fout, numofsamples, basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename, numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult, rotation_angle, basemprimendpts_c, forwardandturncostmult, forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult, interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt, currentangle, numberofprimsperangle, resolution, currentangle_36000int, l, iind, errorxy, interind, endy_c, angleind, endpt - # Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text, int2str, basemprimendpts33p75_c, pause, axis, sin, pinv, basemprimendpts11p25_c, fprintf, fclose, rem, zeros, fopen, round, size - #% - #%generates motion primitives and saves them into file - #% - #%written by Maxim Likhachev - #%--------------------------------------------------- - #% - #%defines - UNICYCLE_MPRIM_16DEGS = 1.0 - if UNICYCLE_MPRIM_16DEGS == 1.0: - resolution = 0.05 - numberofangles = 16 - #%preferably a power of 2, definitely multiple of 8 - numberofprimsperangle = 7 - #%multipliers (multiplier is used as costmult*cost) - forwardcostmult = 1.0 - backwardcostmult = 40.0 - forwardandturncostmult = 2.0 - sidestepcostmult = 10.0 - turninplacecostmult = 20.0 - #%note, what is shown x,y,theta changes (not absolute numbers) - #%0 degreees - basemprimendpts0_c = np.zeros((numberofprimsperangle, 4)) - #%x,y,theta,costmult - #%x aligned with the heading of the robot, angles are positive - #%counterclockwise - #%0 theta change - basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult))) - basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult))) - basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult))) - #%1/16 theta change - basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult))) - basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult))) - #%turn in place - basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult))) - basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult))) - #%45 degrees - basemprimendpts45_c = np.zeros((numberofprimsperangle, 4)) - #%x,y,theta,costmult (multiplier is used as costmult*cost) - #%x aligned with the heading of the robot, angles are positive - #%counterclockwise - #%0 theta change - basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult))) - basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult))) - basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult))) - #%1/16 theta change - basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult))) - basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult))) - #%turn in place - basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult))) - basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult))) - #%22.5 degrees - basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4)) - #%x,y,theta,costmult (multiplier is used as costmult*cost) - #%x aligned with the heading of the robot, angles are positive - #%counterclockwise - #%0 theta change - basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult))) - basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult))) - basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult))) - #%1/16 theta change - basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult))) - basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult))) - #%turn in place - basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult))) - basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult))) - else: - print('ERROR: undefined mprims type\n') - return [] - - fout = open(outfilename, 'w') - #%write the header - fout.write('resolution_m: %f\n' % (resolution)) - fout.write('numberofangles: %d\n' % (numberofangles)) - fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles)) - #%iterate over angles - for angleind in np.arange(1.0, (numberofangles) + 1): - currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles - currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles) - if visualize: - if separate_plots: - fig = plt.figure(angleind) - plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0)) - else: - fig = plt.figure(1) - - plt.axis('equal') - plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution]) - ax = fig.add_subplot(1, 1, 1) - major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution) - minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution) - ax.set_xticks(major_ticks) - ax.set_xticks(minor_ticks, minor=True) - ax.set_yticks(major_ticks) - ax.set_yticks(minor_ticks, minor=True) - ax.grid(which='minor', alpha=0.5) - ax.grid(which='major', alpha=0.9) - - #%iterate over primitives - for primind in np.arange(1.0, (numberofprimsperangle) + 1): - fout.write('primID: %d\n' % (primind - 1)) - fout.write('startangle_c: %d\n' % (angleind - 1)) - #%current angle - #%compute which template to use - if (currentangle_36000int % 9000) == 0: - basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :] - angle = currentangle - elif (currentangle_36000int % 4500) == 0: - basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :] - angle = currentangle - 45.0 * np.pi / 180.0 - - elif ((currentangle_36000int - 7875) % 9000) == 0: - basemprimendpts_c = ( - 1 * basemprimendpts33p75_c[primind, :] - ) # 1* to force deep copy to avoid reference update below - basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1] - #%reverse x and y - basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0] - basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2] - #%reverse the angle as well - angle = currentangle - (78.75 * np.pi) / 180.0 - print('78p75\n') - - elif ((currentangle_36000int - 6750) % 9000) == 0: - basemprimendpts_c = ( - 1 * basemprimendpts22p5_c[int(primind) - 1, :] - ) # 1* to force deep copy to avoid reference update below - basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1] - #%reverse x and y - basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0] - basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2] - #%reverse the angle as well - # print('%d : %d %d %d onto %d %d %d\n'%(primind-1,basemprimendpts22p5_c[int(primind)-1,0], basemprimendpts22p5_c[int(primind)-1,1], basemprimendpts22p5_c[int(primind)-1,2], basemprimendpts_c[0], basemprimendpts_c[1], basemprimendpts_c[2])) - angle = currentangle - (67.5 * np.pi) / 180.0 - print('67p5\n') - - elif ((currentangle_36000int - 5625) % 9000) == 0: - basemprimendpts_c = ( - 1 * basemprimendpts11p25_c[primind, :] - ) # 1* to force deep copy to avoid reference update below - basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1] - #%reverse x and y - basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0] - basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2] - #%reverse the angle as well - angle = currentangle - (56.25 * np.pi) / 180.0 - print('56p25\n') - - elif ((currentangle_36000int - 3375) % 9000) == 0: - basemprimendpts_c = basemprimendpts33p75_c[int(primind), :] - angle = currentangle - (33.75 * np.pi) / 180.0 - print('33p75\n') - - elif ((currentangle_36000int - 2250) % 9000) == 0: - basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :] - angle = currentangle - (22.5 * np.pi) / 180.0 - print('22p5\n') - - elif ((currentangle_36000int - 1125) % 9000) == 0: - basemprimendpts_c = basemprimendpts11p25_c[int(primind), :] - angle = currentangle - (11.25 * np.pi) / 180.0 - print('11p25\n') - - else: - print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int) - return [] - - #%now figure out what action will be - baseendpose_c = basemprimendpts_c[0:3] - additionalactioncostmult = basemprimendpts_c[3] - endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle))) - endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle))) - endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles) - endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c))) - print("endpose_c=", endpose_c) - print(('rotation angle=%f\n' % (angle * 180.0 / np.pi))) - # if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.): - #%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3)); - - #%generate intermediate poses (remember they are w.r.t 0,0 (and not - #%centers of the cells) - numofsamples = 10 - intermcells_m = np.zeros((numofsamples, 3)) - if UNICYCLE_MPRIM_16DEGS == 1.0: - startpt = np.array(np.hstack((0.0, 0.0, currentangle))) - endpt = np.array( - np.hstack( - ( - (endpose_c[0] * resolution), - (endpose_c[1] * resolution), - ( - ((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi) - / numberofangles - ), - ) - ) - ) - - print("startpt =", startpt) - print("endpt =", endpt) - intermcells_m = np.zeros((numofsamples, 3)) - if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0): - #%turn in place or move forward - for iind in np.arange(1.0, (numofsamples) + 1): - fraction = float(iind - 1) / (numofsamples - 1) - intermcells_m[int(iind) - 1, :] = np.array( - ( - startpt[0] + (endpt[0] - startpt[0]) * fraction, - startpt[1] + (endpt[1] - startpt[1]) * fraction, - 0, - ) - ) - rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles) - intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi)) - # print " ",iind," of ",numofsamples," fraction=",fraction," rotation=",rotation_angle - - else: - #%unicycle-based move forward or backward (http://sbpl.net/node/53) - R = np.array( - np.vstack( - ( - np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))), - np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))), - ) - ) - ) - - S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1])))) - l = S[0] - tvoverrv = S[1] - rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv - tv = tvoverrv * rv - - # print "R=\n",R - # print "Rpi=\n",np.linalg.pinv(R) - # print "S=\n",S - # print "l=",l - # print "tvoverrv=",tvoverrv - # print "rv=",rv - # print "tv=",tv - - if l < 0.0: - print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l))) - l = 0.0 - - #%compute rv - #%rv = baseendpose_c(3)*2*pi/numberofangles; - #%compute tv - #%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3))) - #%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3))) - #%tv = (tvx + tvy)/2.0; - #%generate samples - for iind in np.arange(1, numofsamples + 1): - dt = (iind - 1) / (numofsamples - 1) - #%dtheta = rv*dt + startpt(3); - #%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ... - #% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ... - #% dtheta]; - if (dt * tv) < l: - intermcells_m[int(iind) - 1, :] = np.array( - np.hstack( - ( - startpt[0] + dt * tv * np.cos(startpt[2]), - startpt[1] + dt * tv * np.sin(startpt[2]), - startpt[2], - ) - ) - ) - else: - dtheta = rv * (dt - l / tv) + startpt[2] - intermcells_m[int(iind) - 1, :] = np.array( - np.hstack( - ( - startpt[0] - + l * np.cos(startpt[2]) - + tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])), - startpt[1] - + l * np.sin(startpt[2]) - - tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])), - dtheta, - ) - ) - ) - - #%correct - errorxy = np.array( - np.hstack( - ( - endpt[0] - intermcells_m[int(numofsamples) - 1, 0], - endpt[1] - intermcells_m[int(numofsamples) - 1, 1], - ) - ) - ) - # print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1])) - interpfactor = np.array( - np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1)))) - ) - - # print "intermcells_m=",intermcells_m - # print "interp'=",interpfactor.conj().T - - intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T - intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T - - #%write out - fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2])) - fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult)) - fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0))) - for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1): - fout.write( - '%.4f %.4f %.4f\n' - % ( - intermcells_m[int(interind) - 1, 0], - intermcells_m[int(interind) - 1, 1], - intermcells_m[int(interind) - 1, 2], - ) - ) - - if visualize: - plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o") - plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2])) - plt.hold(True) - # if (visualize): - # plt.waitforbuttonpress() # uncomment to plot each primitive set one at a time - - fout.close() - if visualize: - # plt.waitforbuttonpress() # hold until buttom pressed - plt.show() # Keep windows open until the program is terminated - return [] - - -if __name__ == "__main__": - rospack = rospkg.RosPack() - outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim' - genmprim_unicycle(outfilename, visualize=True) diff --git a/mir_navigation/mprim/unicycle_5cm.mprim b/mir_navigation/mprim/unicycle_5cm.mprim deleted file mode 100644 index cb56cd0d..00000000 --- a/mir_navigation/mprim/unicycle_5cm.mprim +++ /dev/null @@ -1,1203 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 80 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0278 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0389 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0500 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 8 1 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 8 -1 -1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0111 -0.0056 0.3927 --0.0222 -0.0111 0.3927 --0.0333 -0.0167 0.3927 --0.0444 -0.0222 0.3927 --0.0556 -0.0278 0.3927 --0.0667 -0.0333 0.3927 --0.0778 -0.0389 0.3927 --0.0889 -0.0444 0.3927 --0.1000 -0.0500 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 7 2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0056 -0.0056 0.7854 --0.0111 -0.0111 0.7854 --0.0167 -0.0167 0.7854 --0.0222 -0.0222 0.7854 --0.0278 -0.0278 0.7854 --0.0333 -0.0333 0.7854 --0.0389 -0.0389 0.7854 --0.0444 -0.0444 0.7854 --0.0500 -0.0500 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 5 7 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 7 5 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0056 -0.0111 1.1781 --0.0111 -0.0222 1.1781 --0.0167 -0.0333 1.1781 --0.0222 -0.0444 1.1781 --0.0278 -0.0556 1.1781 --0.0333 -0.0667 1.1781 --0.0389 -0.0778 1.1781 --0.0444 -0.0889 1.1781 --0.0500 -0.1000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0278 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0389 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0500 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 8 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 1 8 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0000 0.0452 1.5708 --0.0000 0.0904 1.5708 --0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0056 -0.0111 1.9635 -0.0111 -0.0222 1.9635 -0.0167 -0.0333 1.9635 -0.0222 -0.0444 1.9635 -0.0278 -0.0556 1.9635 -0.0333 -0.0667 1.9635 -0.0389 -0.0778 1.9635 -0.0444 -0.0889 1.9635 -0.0500 -0.1000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0056 -0.0056 2.3562 -0.0111 -0.0111 2.3562 -0.0167 -0.0167 2.3562 -0.0222 -0.0222 2.3562 -0.0278 -0.0278 2.3562 -0.0333 -0.0333 2.3562 -0.0389 -0.0389 2.3562 -0.0444 -0.0444 2.3562 -0.0500 -0.0500 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -7 5 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -5 7 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0111 -0.0056 2.7489 -0.0222 -0.0111 2.7489 -0.0333 -0.0167 2.7489 -0.0444 -0.0222 2.7489 -0.0556 -0.0278 2.7489 -0.0667 -0.0333 2.7489 -0.0778 -0.0389 2.7489 -0.0889 -0.0444 2.7489 -0.1000 -0.0500 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -7 2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0278 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0389 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0500 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -8 -1 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -8 1 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0111 0.0056 3.5343 -0.0222 0.0111 3.5343 -0.0333 0.0167 3.5343 -0.0444 0.0222 3.5343 -0.0556 0.0278 3.5343 -0.0667 0.0333 3.5343 -0.0778 0.0389 3.5343 -0.0889 0.0444 3.5343 -0.1000 0.0500 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -7 -2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0056 0.0056 3.9270 -0.0111 0.0111 3.9270 -0.0167 0.0167 3.9270 -0.0222 0.0222 3.9270 -0.0278 0.0278 3.9270 -0.0333 0.0333 3.9270 -0.0389 0.0389 3.9270 -0.0444 0.0444 3.9270 -0.0500 0.0500 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -5 -7 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -7 -5 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0056 0.0111 4.3197 -0.0111 0.0222 4.3197 -0.0167 0.0333 4.3197 -0.0222 0.0444 4.3197 -0.0278 0.0556 4.3197 -0.0333 0.0667 4.3197 -0.0389 0.0778 4.3197 -0.0444 0.0889 4.3197 -0.0500 0.1000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0278 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0389 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0500 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -8 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -1 -8 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0056 0.0111 5.1051 --0.0111 0.0222 5.1051 --0.0167 0.0333 5.1051 --0.0222 0.0444 5.1051 --0.0278 0.0556 5.1051 --0.0333 0.0667 5.1051 --0.0389 0.0778 5.1051 --0.0444 0.0889 5.1051 --0.0500 0.1000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0056 0.0056 5.4978 --0.0111 0.0111 5.4978 --0.0167 0.0167 5.4978 --0.0222 0.0222 5.4978 --0.0278 0.0278 5.4978 --0.0333 0.0333 5.4978 --0.0389 0.0389 5.4978 --0.0444 0.0444 5.4978 --0.0500 0.0500 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 7 -5 15 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 5 -7 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0111 0.0056 5.8905 --0.0222 0.0111 5.8905 --0.0333 0.0167 5.8905 --0.0444 0.0222 5.8905 --0.0556 0.0278 5.8905 --0.0667 0.0333 5.8905 --0.0778 0.0389 5.8905 --0.0889 0.0444 5.8905 --0.1000 0.0500 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 7 -2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 diff --git a/mir_navigation/mprim/unicycle_5cm_expensive_turn_in_place.mprim b/mir_navigation/mprim/unicycle_5cm_expensive_turn_in_place.mprim deleted file mode 100644 index f27907b3..00000000 --- a/mir_navigation/mprim/unicycle_5cm_expensive_turn_in_place.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0278 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0389 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0500 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 8 1 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 8 -1 -1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0111 -0.0056 0.3927 --0.0222 -0.0111 0.3927 --0.0333 -0.0167 0.3927 --0.0444 -0.0222 0.3927 --0.0556 -0.0278 0.3927 --0.0667 -0.0333 0.3927 --0.0778 -0.0389 0.3927 --0.0889 -0.0444 0.3927 --0.1000 -0.0500 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 7 2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0056 -0.0056 0.7854 --0.0111 -0.0111 0.7854 --0.0167 -0.0167 0.7854 --0.0222 -0.0222 0.7854 --0.0278 -0.0278 0.7854 --0.0333 -0.0333 0.7854 --0.0389 -0.0389 0.7854 --0.0444 -0.0444 0.7854 --0.0500 -0.0500 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 5 7 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 7 5 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0056 -0.0111 1.1781 --0.0111 -0.0222 1.1781 --0.0167 -0.0333 1.1781 --0.0222 -0.0444 1.1781 --0.0278 -0.0556 1.1781 --0.0333 -0.0667 1.1781 --0.0389 -0.0778 1.1781 --0.0444 -0.0889 1.1781 --0.0500 -0.1000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0278 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0389 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0500 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 8 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 1 8 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0056 -0.0111 1.9635 -0.0111 -0.0222 1.9635 -0.0167 -0.0333 1.9635 -0.0222 -0.0444 1.9635 -0.0278 -0.0556 1.9635 -0.0333 -0.0667 1.9635 -0.0389 -0.0778 1.9635 -0.0444 -0.0889 1.9635 -0.0500 -0.1000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0056 -0.0056 2.3562 -0.0111 -0.0111 2.3562 -0.0167 -0.0167 2.3562 -0.0222 -0.0222 2.3562 -0.0278 -0.0278 2.3562 -0.0333 -0.0333 2.3562 -0.0389 -0.0389 2.3562 -0.0444 -0.0444 2.3562 -0.0500 -0.0500 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -7 5 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -5 7 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0111 -0.0056 2.7489 -0.0222 -0.0111 2.7489 -0.0333 -0.0167 2.7489 -0.0444 -0.0222 2.7489 -0.0556 -0.0278 2.7489 -0.0667 -0.0333 2.7489 -0.0778 -0.0389 2.7489 -0.0889 -0.0444 2.7489 -0.1000 -0.0500 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -7 2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0278 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0389 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0500 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -8 -1 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -8 1 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0111 0.0056 3.5343 -0.0222 0.0111 3.5343 -0.0333 0.0167 3.5343 -0.0444 0.0222 3.5343 -0.0556 0.0278 3.5343 -0.0667 0.0333 3.5343 -0.0778 0.0389 3.5343 -0.0889 0.0444 3.5343 -0.1000 0.0500 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -7 -2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0056 0.0056 3.9270 -0.0111 0.0111 3.9270 -0.0167 0.0167 3.9270 -0.0222 0.0222 3.9270 -0.0278 0.0278 3.9270 -0.0333 0.0333 3.9270 -0.0389 0.0389 3.9270 -0.0444 0.0444 3.9270 -0.0500 0.0500 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -5 -7 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -7 -5 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0056 0.0111 4.3197 -0.0111 0.0222 4.3197 -0.0167 0.0333 4.3197 -0.0222 0.0444 4.3197 -0.0278 0.0556 4.3197 -0.0333 0.0667 4.3197 -0.0389 0.0778 4.3197 -0.0444 0.0889 4.3197 -0.0500 0.1000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0278 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0389 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0500 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -8 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -1 -8 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0056 0.0111 5.1051 --0.0111 0.0222 5.1051 --0.0167 0.0333 5.1051 --0.0222 0.0444 5.1051 --0.0278 0.0556 5.1051 --0.0333 0.0667 5.1051 --0.0389 0.0778 5.1051 --0.0444 0.0889 5.1051 --0.0500 0.1000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0056 0.0056 5.4978 --0.0111 0.0111 5.4978 --0.0167 0.0167 5.4978 --0.0222 0.0222 5.4978 --0.0278 0.0278 5.4978 --0.0333 0.0333 5.4978 --0.0389 0.0389 5.4978 --0.0444 0.0444 5.4978 --0.0500 0.0500 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 7 -5 15 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 5 -7 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0111 0.0056 5.8905 --0.0222 0.0111 5.8905 --0.0333 0.0167 5.8905 --0.0444 0.0222 5.8905 --0.0556 0.0278 5.8905 --0.0667 0.0333 5.8905 --0.0778 0.0389 5.8905 --0.0889 0.0444 5.8905 --0.1000 0.0500 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 7 -2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/mir_navigation/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim b/mir_navigation/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim deleted file mode 100644 index 0f11e5d5..00000000 --- a/mir_navigation/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0278 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0389 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0500 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 8 1 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 8 -1 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0111 -0.0056 0.3927 --0.0222 -0.0111 0.3927 --0.0333 -0.0167 0.3927 --0.0444 -0.0222 0.3927 --0.0556 -0.0278 0.3927 --0.0667 -0.0333 0.3927 --0.0778 -0.0389 0.3927 --0.0889 -0.0444 0.3927 --0.1000 -0.0500 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 7 2 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0056 -0.0056 0.7854 --0.0111 -0.0111 0.7854 --0.0167 -0.0167 0.7854 --0.0222 -0.0222 0.7854 --0.0278 -0.0278 0.7854 --0.0333 -0.0333 0.7854 --0.0389 -0.0389 0.7854 --0.0444 -0.0444 0.7854 --0.0500 -0.0500 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 5 7 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 7 5 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0056 -0.0111 1.1781 --0.0111 -0.0222 1.1781 --0.0167 -0.0333 1.1781 --0.0222 -0.0444 1.1781 --0.0278 -0.0556 1.1781 --0.0333 -0.0667 1.1781 --0.0389 -0.0778 1.1781 --0.0444 -0.0889 1.1781 --0.0500 -0.1000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 2 7 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0278 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0389 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0500 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 8 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 1 8 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0056 -0.0111 1.9635 -0.0111 -0.0222 1.9635 -0.0167 -0.0333 1.9635 -0.0222 -0.0444 1.9635 -0.0278 -0.0556 1.9635 -0.0333 -0.0667 1.9635 -0.0389 -0.0778 1.9635 -0.0444 -0.0889 1.9635 -0.0500 -0.1000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -2 7 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0056 -0.0056 2.3562 -0.0111 -0.0111 2.3562 -0.0167 -0.0167 2.3562 -0.0222 -0.0222 2.3562 -0.0278 -0.0278 2.3562 -0.0333 -0.0333 2.3562 -0.0389 -0.0389 2.3562 -0.0444 -0.0444 2.3562 -0.0500 -0.0500 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -7 5 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -5 7 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0111 -0.0056 2.7489 -0.0222 -0.0111 2.7489 -0.0333 -0.0167 2.7489 -0.0444 -0.0222 2.7489 -0.0556 -0.0278 2.7489 -0.0667 -0.0333 2.7489 -0.0778 -0.0389 2.7489 -0.0889 -0.0444 2.7489 -0.1000 -0.0500 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -7 2 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0278 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0389 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0500 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -8 -1 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -8 1 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0111 0.0056 3.5343 -0.0222 0.0111 3.5343 -0.0333 0.0167 3.5343 -0.0444 0.0222 3.5343 -0.0556 0.0278 3.5343 -0.0667 0.0333 3.5343 -0.0778 0.0389 3.5343 -0.0889 0.0444 3.5343 -0.1000 0.0500 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -7 -2 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0056 0.0056 3.9270 -0.0111 0.0111 3.9270 -0.0167 0.0167 3.9270 -0.0222 0.0222 3.9270 -0.0278 0.0278 3.9270 -0.0333 0.0333 3.9270 -0.0389 0.0389 3.9270 -0.0444 0.0444 3.9270 -0.0500 0.0500 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -5 -7 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -7 -5 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0056 0.0111 4.3197 -0.0111 0.0222 4.3197 -0.0167 0.0333 4.3197 -0.0222 0.0444 4.3197 -0.0278 0.0556 4.3197 -0.0333 0.0667 4.3197 -0.0389 0.0778 4.3197 -0.0444 0.0889 4.3197 -0.0500 0.1000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -2 -7 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0278 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0389 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0500 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -8 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -1 -8 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0056 0.0111 5.1051 --0.0111 0.0222 5.1051 --0.0167 0.0333 5.1051 --0.0222 0.0444 5.1051 --0.0278 0.0556 5.1051 --0.0333 0.0667 5.1051 --0.0389 0.0778 5.1051 --0.0444 0.0889 5.1051 --0.0500 0.1000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 2 -7 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0056 0.0056 5.4978 --0.0111 0.0111 5.4978 --0.0167 0.0167 5.4978 --0.0222 0.0222 5.4978 --0.0278 0.0278 5.4978 --0.0333 0.0333 5.4978 --0.0389 0.0389 5.4978 --0.0444 0.0444 5.4978 --0.0500 0.0500 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 7 -5 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 5 -7 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0111 0.0056 5.8905 --0.0222 0.0111 5.8905 --0.0333 0.0167 5.8905 --0.0444 0.0222 5.8905 --0.0556 0.0278 5.8905 --0.0667 0.0333 5.8905 --0.0778 0.0389 5.8905 --0.0889 0.0444 5.8905 --0.1000 0.0500 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 7 -2 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/mir_navigation/mprim/unicycle_5cm_noreverse_trolley.mprim b/mir_navigation/mprim/unicycle_5cm_noreverse_trolley.mprim deleted file mode 100644 index 596f5c54..00000000 --- a/mir_navigation/mprim/unicycle_5cm_noreverse_trolley.mprim +++ /dev/null @@ -1,2403 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 160 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: 16 4 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0904 0.0095 0.0349 -0.1806 0.0222 0.0699 -0.2707 0.0381 0.1048 -0.3604 0.0572 0.1398 -0.4496 0.0795 0.1747 -0.5383 0.1050 0.2097 -0.6264 0.1335 0.2446 -0.7136 0.1652 0.2796 -0.8000 0.2000 0.3145 -primID: 3 -startangle_c: 0 -endpose_c: 16 -4 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0904 -0.0095 -0.0349 -0.1806 -0.0222 -0.0699 -0.2707 -0.0381 -0.1048 -0.3604 -0.0572 -0.1398 -0.4496 -0.0795 -0.1747 -0.5383 -0.1050 -0.2097 -0.6264 -0.1335 -0.2446 -0.7136 -0.1652 -0.2796 -0.8000 -0.2000 -0.3145 -primID: 4 -startangle_c: 0 -endpose_c: 5 2 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0280 0.0085 0.0223 -0.0559 0.0177 0.0445 -0.0839 0.0275 0.0668 -0.1117 0.0380 0.0890 -0.1396 0.0491 0.1113 -0.1673 0.0609 0.1335 -0.1950 0.0733 0.1558 -0.2225 0.0863 0.1781 -0.2500 0.1000 0.2003 -primID: 5 -startangle_c: 0 -endpose_c: 5 -2 -1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0280 -0.0085 -0.0223 -0.0559 -0.0177 -0.0445 -0.0839 -0.0275 -0.0668 -0.1117 -0.0380 -0.0890 -0.1396 -0.0491 -0.1113 -0.1673 -0.0609 -0.1335 -0.1950 -0.0733 -0.1558 -0.2225 -0.0863 -0.1781 -0.2500 -0.1000 -0.2003 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 7 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 8 -startangle_c: 0 -endpose_c: 8 1 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0056 0.0000 -0.0889 0.0111 0.0000 -0.1333 0.0167 0.0000 -0.1778 0.0222 0.0000 -0.2222 0.0278 0.0000 -0.2667 0.0333 0.0000 -0.3111 0.0389 0.0000 -0.3556 0.0444 0.0000 -0.4000 0.0500 0.0000 -primID: 9 -startangle_c: 0 -endpose_c: 8 -1 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 -0.0056 0.0000 -0.0889 -0.0111 0.0000 -0.1333 -0.0167 0.0000 -0.1778 -0.0222 0.0000 -0.2222 -0.0278 0.0000 -0.2667 -0.0333 0.0000 -0.3111 -0.0389 0.0000 -0.3556 -0.0444 0.0000 -0.4000 -0.0500 0.0000 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: 13 9 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0793 0.0378 0.4329 -0.1572 0.0787 0.4732 -0.2334 0.1229 0.5134 -0.3079 0.1701 0.5537 -0.3805 0.2204 0.5939 -0.4512 0.2736 0.6342 -0.5197 0.3296 0.6744 -0.5860 0.3885 0.7147 -0.6500 0.4500 0.7549 -primID: 3 -startangle_c: 1 -endpose_c: 14 3 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0741 0.0305 0.3774 -0.1493 0.0582 0.3302 -0.2256 0.0824 0.2830 -0.3031 0.1030 0.2359 -0.3814 0.1199 0.1887 -0.4604 0.1330 0.1415 -0.5400 0.1424 0.0943 -0.6199 0.1481 0.0472 -0.7000 0.1500 -0.0000 -primID: 4 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 5 -startangle_c: 1 -endpose_c: 6 1 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0320 0.0105 0.3551 -0.0644 0.0197 0.3174 -0.0973 0.0278 0.2798 -0.1304 0.0346 0.2421 -0.1639 0.0402 0.2045 -0.1977 0.0446 0.1669 -0.2316 0.0476 0.1292 -0.2658 0.0495 0.0916 -0.3000 0.0500 0.0540 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 7 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 8 -startangle_c: 1 -endpose_c: 7 2 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0389 0.0111 0.3927 -0.0778 0.0222 0.3927 -0.1167 0.0333 0.3927 -0.1556 0.0444 0.3927 -0.1944 0.0556 0.3927 -0.2333 0.0667 0.3927 -0.2722 0.0778 0.3927 -0.3111 0.0889 0.3927 -0.3500 0.1000 0.3927 -primID: 9 -startangle_c: 1 -endpose_c: 5 4 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0278 0.0222 0.3927 -0.0556 0.0444 0.3927 -0.0833 0.0667 0.3927 -0.1111 0.0889 0.3927 -0.1389 0.1111 0.3927 -0.1667 0.1333 0.3927 -0.1944 0.1556 0.3927 -0.2222 0.1778 0.3927 -0.2500 0.2000 0.3927 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: 11 14 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0705 0.0705 0.7854 -0.1411 0.1411 0.7854 -0.2116 0.2116 0.7854 -0.2816 0.2828 0.8201 -0.3469 0.3581 0.8917 -0.4068 0.4379 0.9633 -0.4607 0.5218 1.0349 -0.5086 0.6093 1.1065 -0.5500 0.7000 1.1781 -primID: 3 -startangle_c: 2 -endpose_c: 14 11 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0705 0.0705 0.7854 -0.1411 0.1411 0.7854 -0.2116 0.2116 0.7854 -0.2828 0.2816 0.7507 -0.3581 0.3469 0.6791 -0.4379 0.4068 0.6075 -0.5218 0.4607 0.5359 -0.6093 0.5086 0.4643 -0.7000 0.5500 0.3927 -primID: 4 -startangle_c: 2 -endpose_c: 4 6 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0278 0.0292 0.8288 -0.0543 0.0595 0.8722 -0.0795 0.0910 0.9156 -0.1033 0.1235 0.9590 -0.1257 0.1571 1.0024 -0.1466 0.1916 1.0458 -0.1659 0.2269 1.0892 -0.1838 0.2631 1.1326 -0.2000 0.3000 1.1760 -primID: 5 -startangle_c: 2 -endpose_c: 6 4 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0292 0.0278 0.7420 -0.0595 0.0543 0.6986 -0.0910 0.0795 0.6552 -0.1235 0.1033 0.6118 -0.1571 0.1257 0.5684 -0.1916 0.1466 0.5250 -0.2269 0.1659 0.4816 -0.2631 0.1838 0.4382 -0.3000 0.2000 0.3948 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 7 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 8 -startangle_c: 2 -endpose_c: 6 7 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0389 0.7854 -0.0667 0.0778 0.7854 -0.1000 0.1167 0.7854 -0.1333 0.1556 0.7854 -0.1667 0.1944 0.7854 -0.2000 0.2333 0.7854 -0.2333 0.2722 0.7854 -0.2667 0.3111 0.7854 -0.3000 0.3500 0.7854 -primID: 9 -startangle_c: 2 -endpose_c: 7 6 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0389 0.0333 0.7854 -0.0778 0.0667 0.7854 -0.1167 0.1000 0.7854 -0.1556 0.1333 0.7854 -0.1944 0.1667 0.7854 -0.2333 0.2000 0.7854 -0.2722 0.2333 0.7854 -0.3111 0.2667 0.7854 -0.3500 0.3000 0.7854 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: 9 13 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0378 0.0793 1.1378 -0.0787 0.1572 1.0976 -0.1229 0.2334 1.0574 -0.1701 0.3079 1.0171 -0.2204 0.3805 0.9769 -0.2736 0.4512 0.9366 -0.3296 0.5197 0.8964 -0.3885 0.5860 0.8561 -0.4500 0.6500 0.8159 -primID: 3 -startangle_c: 3 -endpose_c: 3 14 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0305 0.0741 1.1934 -0.0582 0.1493 1.2406 -0.0824 0.2256 1.2878 -0.1030 0.3031 1.3349 -0.1199 0.3814 1.3821 -0.1330 0.4604 1.4293 -0.1424 0.5400 1.4765 -0.1481 0.6199 1.5236 -0.1500 0.7000 1.5708 -primID: 4 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 5 -startangle_c: 3 -endpose_c: 1 6 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0105 0.0320 1.2157 -0.0197 0.0644 1.2534 -0.0278 0.0973 1.2910 -0.0346 0.1304 1.3286 -0.0402 0.1639 1.3663 -0.0446 0.1977 1.4039 -0.0476 0.2316 1.4416 -0.0495 0.2658 1.4792 -0.0500 0.3000 1.5168 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 7 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 8 -startangle_c: 3 -endpose_c: 2 7 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0111 0.0389 1.1781 -0.0222 0.0778 1.1781 -0.0333 0.1167 1.1781 -0.0444 0.1556 1.1781 -0.0556 0.1944 1.1781 -0.0667 0.2333 1.1781 -0.0778 0.2722 1.1781 -0.0889 0.3111 1.1781 -0.1000 0.3500 1.1781 -primID: 9 -startangle_c: 3 -endpose_c: 4 5 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0222 0.0278 1.1781 -0.0444 0.0556 1.1781 -0.0667 0.0833 1.1781 -0.0889 0.1111 1.1781 -0.1111 0.1389 1.1781 -0.1333 0.1667 1.1781 -0.1556 0.1944 1.1781 -0.1778 0.2222 1.1781 -0.2000 0.2500 1.1781 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: -4 16 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0095 0.0904 1.6057 --0.0222 0.1806 1.6407 --0.0381 0.2707 1.6756 --0.0572 0.3604 1.7106 --0.0795 0.4496 1.7455 --0.1050 0.5383 1.7805 --0.1335 0.6264 1.8154 --0.1652 0.7136 1.8503 --0.2000 0.8000 1.8853 -primID: 3 -startangle_c: 4 -endpose_c: 4 16 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0095 0.0904 1.5359 -0.0222 0.1806 1.5009 -0.0381 0.2707 1.4660 -0.0572 0.3604 1.4310 -0.0795 0.4496 1.3961 -0.1050 0.5383 1.3611 -0.1335 0.6264 1.3262 -0.1652 0.7136 1.2912 -0.2000 0.8000 1.2563 -primID: 4 -startangle_c: 4 -endpose_c: -2 5 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0085 0.0280 1.5931 --0.0177 0.0559 1.6153 --0.0275 0.0839 1.6376 --0.0380 0.1117 1.6598 --0.0491 0.1396 1.6821 --0.0609 0.1673 1.7043 --0.0733 0.1950 1.7266 --0.0863 0.2225 1.7489 --0.1000 0.2500 1.7711 -primID: 5 -startangle_c: 4 -endpose_c: 2 5 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0085 0.0280 1.5485 -0.0177 0.0559 1.5263 -0.0275 0.0839 1.5040 -0.0380 0.1117 1.4818 -0.0491 0.1396 1.4595 -0.0609 0.1673 1.4373 -0.0733 0.1950 1.4150 -0.0863 0.2225 1.3927 -0.1000 0.2500 1.3705 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 7 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 8 -startangle_c: 4 -endpose_c: -1 8 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0056 0.0444 1.5708 --0.0111 0.0889 1.5708 --0.0167 0.1333 1.5708 --0.0222 0.1778 1.5708 --0.0278 0.2222 1.5708 --0.0333 0.2667 1.5708 --0.0389 0.3111 1.5708 --0.0444 0.3556 1.5708 --0.0500 0.4000 1.5708 -primID: 9 -startangle_c: 4 -endpose_c: 1 8 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0056 0.0444 1.5708 -0.0111 0.0889 1.5708 -0.0167 0.1333 1.5708 -0.0222 0.1778 1.5708 -0.0278 0.2222 1.5708 -0.0333 0.2667 1.5708 -0.0389 0.3111 1.5708 -0.0444 0.3556 1.5708 -0.0500 0.4000 1.5708 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: -9 13 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0378 0.0793 2.0037 --0.0787 0.1572 2.0440 --0.1229 0.2334 2.0842 --0.1701 0.3079 2.1245 --0.2204 0.3805 2.1647 --0.2736 0.4512 2.2050 --0.3296 0.5197 2.2452 --0.3885 0.5860 2.2855 --0.4500 0.6500 2.3257 -primID: 3 -startangle_c: 5 -endpose_c: -3 14 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0305 0.0741 1.9482 --0.0582 0.1493 1.9010 --0.0824 0.2256 1.8538 --0.1030 0.3031 1.8067 --0.1199 0.3814 1.7595 --0.1330 0.4604 1.7123 --0.1424 0.5400 1.6651 --0.1481 0.6199 1.6180 --0.1500 0.7000 1.5708 -primID: 4 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 5 -startangle_c: 5 -endpose_c: -1 6 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0105 0.0320 1.9259 --0.0197 0.0644 1.8882 --0.0278 0.0973 1.8506 --0.0346 0.1304 1.8129 --0.0402 0.1639 1.7753 --0.0446 0.1977 1.7377 --0.0476 0.2316 1.7000 --0.0495 0.2658 1.6624 --0.0500 0.3000 1.6248 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 7 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 8 -startangle_c: 5 -endpose_c: -2 7 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0111 0.0389 1.9635 --0.0222 0.0778 1.9635 --0.0333 0.1167 1.9635 --0.0444 0.1556 1.9635 --0.0556 0.1944 1.9635 --0.0667 0.2333 1.9635 --0.0778 0.2722 1.9635 --0.0889 0.3111 1.9635 --0.1000 0.3500 1.9635 -primID: 9 -startangle_c: 5 -endpose_c: -4 5 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0222 0.0278 1.9635 --0.0444 0.0556 1.9635 --0.0667 0.0833 1.9635 --0.0889 0.1111 1.9635 --0.1111 0.1389 1.9635 --0.1333 0.1667 1.9635 --0.1556 0.1944 1.9635 --0.1778 0.2222 1.9635 --0.2000 0.2500 1.9635 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: -14 11 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0705 0.0705 2.3562 --0.1411 0.1411 2.3562 --0.2116 0.2116 2.3562 --0.2828 0.2816 2.3909 --0.3581 0.3469 2.4625 --0.4379 0.4068 2.5341 --0.5218 0.4607 2.6057 --0.6093 0.5086 2.6773 --0.7000 0.5500 2.7489 -primID: 3 -startangle_c: 6 -endpose_c: -11 14 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0705 0.0705 2.3562 --0.1411 0.1411 2.3562 --0.2116 0.2116 2.3562 --0.2816 0.2828 2.3215 --0.3469 0.3581 2.2499 --0.4068 0.4379 2.1783 --0.4607 0.5218 2.1067 --0.5086 0.6093 2.0351 --0.5500 0.7000 1.9635 -primID: 4 -startangle_c: 6 -endpose_c: -6 4 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0292 0.0278 2.3996 --0.0595 0.0543 2.4430 --0.0910 0.0795 2.4864 --0.1235 0.1033 2.5298 --0.1571 0.1257 2.5732 --0.1916 0.1466 2.6166 --0.2269 0.1659 2.6600 --0.2631 0.1838 2.7034 --0.3000 0.2000 2.7468 -primID: 5 -startangle_c: 6 -endpose_c: -4 6 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0278 0.0292 2.3128 --0.0543 0.0595 2.2694 --0.0795 0.0910 2.2260 --0.1033 0.1235 2.1826 --0.1257 0.1571 2.1392 --0.1466 0.1916 2.0958 --0.1659 0.2269 2.0524 --0.1838 0.2631 2.0090 --0.2000 0.3000 1.9656 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 7 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 8 -startangle_c: 6 -endpose_c: -7 6 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0389 0.0333 2.3562 --0.0778 0.0667 2.3562 --0.1167 0.1000 2.3562 --0.1556 0.1333 2.3562 --0.1944 0.1667 2.3562 --0.2333 0.2000 2.3562 --0.2722 0.2333 2.3562 --0.3111 0.2667 2.3562 --0.3500 0.3000 2.3562 -primID: 9 -startangle_c: 6 -endpose_c: -6 7 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0389 2.3562 --0.0667 0.0778 2.3562 --0.1000 0.1167 2.3562 --0.1333 0.1556 2.3562 --0.1667 0.1944 2.3562 --0.2000 0.2333 2.3562 --0.2333 0.2722 2.3562 --0.2667 0.3111 2.3562 --0.3000 0.3500 2.3562 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: -13 9 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0793 0.0378 2.7086 --0.1572 0.0787 2.6684 --0.2334 0.1229 2.6281 --0.3079 0.1701 2.5879 --0.3805 0.2204 2.5477 --0.4512 0.2736 2.5074 --0.5197 0.3296 2.4672 --0.5860 0.3885 2.4269 --0.6500 0.4500 2.3867 -primID: 3 -startangle_c: 7 -endpose_c: -14 3 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0741 0.0305 2.7642 --0.1493 0.0582 2.8114 --0.2256 0.0824 2.8586 --0.3031 0.1030 2.9057 --0.3814 0.1199 2.9529 --0.4604 0.1330 3.0001 --0.5400 0.1424 3.0472 --0.6199 0.1481 3.0944 --0.7000 0.1500 3.1416 -primID: 4 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 5 -startangle_c: 7 -endpose_c: -6 1 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0320 0.0105 2.7865 --0.0644 0.0197 2.8242 --0.0973 0.0278 2.8618 --0.1304 0.0346 2.8994 --0.1639 0.0402 2.9371 --0.1977 0.0446 2.9747 --0.2316 0.0476 3.0124 --0.2658 0.0495 3.0500 --0.3000 0.0500 3.0876 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 7 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 8 -startangle_c: 7 -endpose_c: -7 2 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0389 0.0111 2.7489 --0.0778 0.0222 2.7489 --0.1167 0.0333 2.7489 --0.1556 0.0444 2.7489 --0.1944 0.0556 2.7489 --0.2333 0.0667 2.7489 --0.2722 0.0778 2.7489 --0.3111 0.0889 2.7489 --0.3500 0.1000 2.7489 -primID: 9 -startangle_c: 7 -endpose_c: -5 4 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0278 0.0222 2.7489 --0.0556 0.0444 2.7489 --0.0833 0.0667 2.7489 --0.1111 0.0889 2.7489 --0.1389 0.1111 2.7489 --0.1667 0.1333 2.7489 --0.1944 0.1556 2.7489 --0.2222 0.1778 2.7489 --0.2500 0.2000 2.7489 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: -16 -4 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0904 -0.0095 3.1765 --0.1806 -0.0222 3.2115 --0.2707 -0.0381 3.2464 --0.3604 -0.0572 3.2814 --0.4496 -0.0795 3.3163 --0.5383 -0.1050 3.3513 --0.6264 -0.1335 3.3862 --0.7136 -0.1652 3.4211 --0.8000 -0.2000 3.4561 -primID: 3 -startangle_c: 8 -endpose_c: -16 4 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0904 0.0095 3.1066 --0.1806 0.0222 3.0717 --0.2707 0.0381 3.0368 --0.3604 0.0572 3.0018 --0.4496 0.0795 2.9669 --0.5383 0.1050 2.9319 --0.6264 0.1335 2.8970 --0.7136 0.1652 2.8620 --0.8000 0.2000 2.8271 -primID: 4 -startangle_c: 8 -endpose_c: -5 -2 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0280 -0.0085 3.1639 --0.0559 -0.0177 3.1861 --0.0839 -0.0275 3.2084 --0.1117 -0.0380 3.2306 --0.1396 -0.0491 3.2529 --0.1673 -0.0609 3.2751 --0.1950 -0.0733 3.2974 --0.2225 -0.0863 3.3197 --0.2500 -0.1000 3.3419 -primID: 5 -startangle_c: 8 -endpose_c: -5 2 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0280 0.0085 3.1193 --0.0559 0.0177 3.0971 --0.0839 0.0275 3.0748 --0.1117 0.0380 3.0526 --0.1396 0.0491 3.0303 --0.1673 0.0609 3.0080 --0.1950 0.0733 2.9858 --0.2225 0.0863 2.9635 --0.2500 0.1000 2.9413 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 7 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 8 -startangle_c: 8 -endpose_c: -8 -1 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 -0.0056 3.1416 --0.0889 -0.0111 3.1416 --0.1333 -0.0167 3.1416 --0.1778 -0.0222 3.1416 --0.2222 -0.0278 3.1416 --0.2667 -0.0333 3.1416 --0.3111 -0.0389 3.1416 --0.3556 -0.0444 3.1416 --0.4000 -0.0500 3.1416 -primID: 9 -startangle_c: 8 -endpose_c: -8 1 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0056 3.1416 --0.0889 0.0111 3.1416 --0.1333 0.0167 3.1416 --0.1778 0.0222 3.1416 --0.2222 0.0278 3.1416 --0.2667 0.0333 3.1416 --0.3111 0.0389 3.1416 --0.3556 0.0444 3.1416 --0.4000 0.0500 3.1416 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: -13 -9 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0793 -0.0378 3.5745 --0.1572 -0.0787 3.6148 --0.2334 -0.1229 3.6550 --0.3079 -0.1701 3.6953 --0.3805 -0.2204 3.7355 --0.4512 -0.2736 3.7758 --0.5197 -0.3296 3.8160 --0.5860 -0.3885 3.8563 --0.6500 -0.4500 3.8965 -primID: 3 -startangle_c: 9 -endpose_c: -14 -3 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0741 -0.0305 3.5190 --0.1493 -0.0582 3.4718 --0.2256 -0.0824 3.4246 --0.3031 -0.1030 3.3775 --0.3814 -0.1199 3.3303 --0.4604 -0.1330 3.2831 --0.5400 -0.1424 3.2359 --0.6199 -0.1481 3.1888 --0.7000 -0.1500 3.1416 -primID: 4 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 5 -startangle_c: 9 -endpose_c: -6 -1 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0320 -0.0105 3.4967 --0.0644 -0.0197 3.4590 --0.0973 -0.0278 3.4214 --0.1304 -0.0346 3.3837 --0.1639 -0.0402 3.3461 --0.1977 -0.0446 3.3085 --0.2316 -0.0476 3.2708 --0.2658 -0.0495 3.2332 --0.3000 -0.0500 3.1955 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 7 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 8 -startangle_c: 9 -endpose_c: -7 -2 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0389 -0.0111 3.5343 --0.0778 -0.0222 3.5343 --0.1167 -0.0333 3.5343 --0.1556 -0.0444 3.5343 --0.1944 -0.0556 3.5343 --0.2333 -0.0667 3.5343 --0.2722 -0.0778 3.5343 --0.3111 -0.0889 3.5343 --0.3500 -0.1000 3.5343 -primID: 9 -startangle_c: 9 -endpose_c: -5 -4 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0278 -0.0222 3.5343 --0.0556 -0.0444 3.5343 --0.0833 -0.0667 3.5343 --0.1111 -0.0889 3.5343 --0.1389 -0.1111 3.5343 --0.1667 -0.1333 3.5343 --0.1944 -0.1556 3.5343 --0.2222 -0.1778 3.5343 --0.2500 -0.2000 3.5343 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: -11 -14 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0705 -0.0705 3.9270 --0.1411 -0.1411 3.9270 --0.2116 -0.2116 3.9270 --0.2816 -0.2828 3.9617 --0.3469 -0.3581 4.0333 --0.4068 -0.4379 4.1049 --0.4607 -0.5218 4.1765 --0.5086 -0.6093 4.2481 --0.5500 -0.7000 4.3197 -primID: 3 -startangle_c: 10 -endpose_c: -14 -11 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0705 -0.0705 3.9270 --0.1411 -0.1411 3.9270 --0.2116 -0.2116 3.9270 --0.2828 -0.2816 3.8923 --0.3581 -0.3469 3.8207 --0.4379 -0.4068 3.7491 --0.5218 -0.4607 3.6775 --0.6093 -0.5086 3.6059 --0.7000 -0.5500 3.5343 -primID: 4 -startangle_c: 10 -endpose_c: -4 -6 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0278 -0.0292 3.9704 --0.0543 -0.0595 4.0138 --0.0795 -0.0910 4.0572 --0.1033 -0.1235 4.1006 --0.1257 -0.1571 4.1440 --0.1466 -0.1916 4.1874 --0.1659 -0.2269 4.2308 --0.1838 -0.2631 4.2742 --0.2000 -0.3000 4.3176 -primID: 5 -startangle_c: 10 -endpose_c: -6 -4 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0292 -0.0278 3.8836 --0.0595 -0.0543 3.8402 --0.0910 -0.0795 3.7968 --0.1235 -0.1033 3.7534 --0.1571 -0.1257 3.7100 --0.1916 -0.1466 3.6666 --0.2269 -0.1659 3.6232 --0.2631 -0.1838 3.5798 --0.3000 -0.2000 3.5364 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 7 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 8 -startangle_c: 10 -endpose_c: -6 -7 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0389 3.9270 --0.0667 -0.0778 3.9270 --0.1000 -0.1167 3.9270 --0.1333 -0.1556 3.9270 --0.1667 -0.1944 3.9270 --0.2000 -0.2333 3.9270 --0.2333 -0.2722 3.9270 --0.2667 -0.3111 3.9270 --0.3000 -0.3500 3.9270 -primID: 9 -startangle_c: 10 -endpose_c: -7 -6 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0389 -0.0333 3.9270 --0.0778 -0.0667 3.9270 --0.1167 -0.1000 3.9270 --0.1556 -0.1333 3.9270 --0.1944 -0.1667 3.9270 --0.2333 -0.2000 3.9270 --0.2722 -0.2333 3.9270 --0.3111 -0.2667 3.9270 --0.3500 -0.3000 3.9270 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: -9 -13 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0378 -0.0793 4.2794 --0.0787 -0.1572 4.2392 --0.1229 -0.2334 4.1989 --0.1701 -0.3079 4.1587 --0.2204 -0.3805 4.1185 --0.2736 -0.4512 4.0782 --0.3296 -0.5197 4.0380 --0.3885 -0.5860 3.9977 --0.4500 -0.6500 3.9575 -primID: 3 -startangle_c: 11 -endpose_c: -3 -14 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0305 -0.0741 4.3350 --0.0582 -0.1493 4.3822 --0.0824 -0.2256 4.4294 --0.1030 -0.3031 4.4765 --0.1199 -0.3814 4.5237 --0.1330 -0.4604 4.5709 --0.1424 -0.5400 4.6180 --0.1481 -0.6199 4.6652 --0.1500 -0.7000 4.7124 -primID: 4 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 5 -startangle_c: 11 -endpose_c: -1 -6 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0105 -0.0320 4.3573 --0.0197 -0.0644 4.3950 --0.0278 -0.0973 4.4326 --0.0346 -0.1304 4.4702 --0.0402 -0.1639 4.5079 --0.0446 -0.1977 4.5455 --0.0476 -0.2316 4.5832 --0.0495 -0.2658 4.6208 --0.0500 -0.3000 4.6584 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 7 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 8 -startangle_c: 11 -endpose_c: -2 -7 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0111 -0.0389 4.3197 --0.0222 -0.0778 4.3197 --0.0333 -0.1167 4.3197 --0.0444 -0.1556 4.3197 --0.0556 -0.1944 4.3197 --0.0667 -0.2333 4.3197 --0.0778 -0.2722 4.3197 --0.0889 -0.3111 4.3197 --0.1000 -0.3500 4.3197 -primID: 9 -startangle_c: 11 -endpose_c: -4 -5 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0222 -0.0278 4.3197 --0.0444 -0.0556 4.3197 --0.0667 -0.0833 4.3197 --0.0889 -0.1111 4.3197 --0.1111 -0.1389 4.3197 --0.1333 -0.1667 4.3197 --0.1556 -0.1944 4.3197 --0.1778 -0.2222 4.3197 --0.2000 -0.2500 4.3197 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 4 -16 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0095 -0.0904 4.7473 -0.0222 -0.1806 4.7823 -0.0381 -0.2707 4.8172 -0.0572 -0.3604 4.8522 -0.0795 -0.4496 4.8871 -0.1050 -0.5383 4.9221 -0.1335 -0.6264 4.9570 -0.1652 -0.7136 4.9919 -0.2000 -0.8000 5.0269 -primID: 3 -startangle_c: 12 -endpose_c: -4 -16 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0095 -0.0904 4.6774 --0.0222 -0.1806 4.6425 --0.0381 -0.2707 4.6076 --0.0572 -0.3604 4.5726 --0.0795 -0.4496 4.5377 --0.1050 -0.5383 4.5027 --0.1335 -0.6264 4.4678 --0.1652 -0.7136 4.4328 --0.2000 -0.8000 4.3979 -primID: 4 -startangle_c: 12 -endpose_c: 2 -5 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0085 -0.0280 4.7346 -0.0177 -0.0559 4.7569 -0.0275 -0.0839 4.7792 -0.0380 -0.1117 4.8014 -0.0491 -0.1396 4.8237 -0.0609 -0.1673 4.8459 -0.0733 -0.1950 4.8682 -0.0863 -0.2225 4.8904 -0.1000 -0.2500 4.9127 -primID: 5 -startangle_c: 12 -endpose_c: -2 -5 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0085 -0.0280 4.6901 --0.0177 -0.0559 4.6679 --0.0275 -0.0839 4.6456 --0.0380 -0.1117 4.6234 --0.0491 -0.1396 4.6011 --0.0609 -0.1673 4.5788 --0.0733 -0.1950 4.5566 --0.0863 -0.2225 4.5343 --0.1000 -0.2500 4.5121 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 7 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 8 -startangle_c: 12 -endpose_c: 1 -8 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0056 -0.0444 4.7124 -0.0111 -0.0889 4.7124 -0.0167 -0.1333 4.7124 -0.0222 -0.1778 4.7124 -0.0278 -0.2222 4.7124 -0.0333 -0.2667 4.7124 -0.0389 -0.3111 4.7124 -0.0444 -0.3556 4.7124 -0.0500 -0.4000 4.7124 -primID: 9 -startangle_c: 12 -endpose_c: -1 -8 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0056 -0.0444 4.7124 --0.0111 -0.0889 4.7124 --0.0167 -0.1333 4.7124 --0.0222 -0.1778 4.7124 --0.0278 -0.2222 4.7124 --0.0333 -0.2667 4.7124 --0.0389 -0.3111 4.7124 --0.0444 -0.3556 4.7124 --0.0500 -0.4000 4.7124 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: 9 -13 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0378 -0.0793 5.1453 -0.0787 -0.1572 5.1856 -0.1229 -0.2334 5.2258 -0.1701 -0.3079 5.2661 -0.2204 -0.3805 5.3063 -0.2736 -0.4512 5.3466 -0.3296 -0.5197 5.3868 -0.3885 -0.5860 5.4271 -0.4500 -0.6500 5.4673 -primID: 3 -startangle_c: 13 -endpose_c: 3 -14 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0305 -0.0741 5.0898 -0.0582 -0.1493 5.0426 -0.0824 -0.2256 4.9954 -0.1030 -0.3031 4.9482 -0.1199 -0.3814 4.9011 -0.1330 -0.4604 4.8539 -0.1424 -0.5400 4.8067 -0.1481 -0.6199 4.7596 -0.1500 -0.7000 4.7124 -primID: 4 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 5 -startangle_c: 13 -endpose_c: 1 -6 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0105 -0.0320 5.0674 -0.0197 -0.0644 5.0298 -0.0278 -0.0973 4.9922 -0.0346 -0.1304 4.9545 -0.0402 -0.1639 4.9169 -0.0446 -0.1977 4.8793 -0.0476 -0.2316 4.8416 -0.0495 -0.2658 4.8040 -0.0500 -0.3000 4.7663 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 7 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 8 -startangle_c: 13 -endpose_c: 2 -7 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0111 -0.0389 5.1051 -0.0222 -0.0778 5.1051 -0.0333 -0.1167 5.1051 -0.0444 -0.1556 5.1051 -0.0556 -0.1944 5.1051 -0.0667 -0.2333 5.1051 -0.0778 -0.2722 5.1051 -0.0889 -0.3111 5.1051 -0.1000 -0.3500 5.1051 -primID: 9 -startangle_c: 13 -endpose_c: 4 -5 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0222 -0.0278 5.1051 -0.0444 -0.0556 5.1051 -0.0667 -0.0833 5.1051 -0.0889 -0.1111 5.1051 -0.1111 -0.1389 5.1051 -0.1333 -0.1667 5.1051 -0.1556 -0.1944 5.1051 -0.1778 -0.2222 5.1051 -0.2000 -0.2500 5.1051 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: 14 -11 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0705 -0.0705 5.4978 -0.1411 -0.1411 5.4978 -0.2116 -0.2116 5.4978 -0.2828 -0.2816 5.5325 -0.3581 -0.3469 5.6041 -0.4379 -0.4068 5.6757 -0.5218 -0.4607 5.7473 -0.6093 -0.5086 5.8189 -0.7000 -0.5500 5.8905 -primID: 3 -startangle_c: 14 -endpose_c: 11 -14 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0705 -0.0705 5.4978 -0.1411 -0.1411 5.4978 -0.2116 -0.2116 5.4978 -0.2816 -0.2828 5.4631 -0.3469 -0.3581 5.3915 -0.4068 -0.4379 5.3199 -0.4607 -0.5218 5.2483 -0.5086 -0.6093 5.1767 -0.5500 -0.7000 5.1051 -primID: 4 -startangle_c: 14 -endpose_c: 6 -4 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0292 -0.0278 5.5412 -0.0595 -0.0543 5.5846 -0.0910 -0.0795 5.6280 -0.1235 -0.1033 5.6714 -0.1571 -0.1257 5.7148 -0.1916 -0.1466 5.7582 -0.2269 -0.1659 5.8016 -0.2631 -0.1838 5.8450 -0.3000 -0.2000 5.8884 -primID: 5 -startangle_c: 14 -endpose_c: 4 -6 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0278 -0.0292 5.4544 -0.0543 -0.0595 5.4110 -0.0795 -0.0910 5.3676 -0.1033 -0.1235 5.3242 -0.1257 -0.1571 5.2808 -0.1466 -0.1916 5.2374 -0.1659 -0.2269 5.1940 -0.1838 -0.2631 5.1506 -0.2000 -0.3000 5.1072 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 7 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 8 -startangle_c: 14 -endpose_c: 7 -6 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0389 -0.0333 5.4978 -0.0778 -0.0667 5.4978 -0.1167 -0.1000 5.4978 -0.1556 -0.1333 5.4978 -0.1944 -0.1667 5.4978 -0.2333 -0.2000 5.4978 -0.2722 -0.2333 5.4978 -0.3111 -0.2667 5.4978 -0.3500 -0.3000 5.4978 -primID: 9 -startangle_c: 14 -endpose_c: 6 -7 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0389 5.4978 -0.0667 -0.0778 5.4978 -0.1000 -0.1167 5.4978 -0.1333 -0.1556 5.4978 -0.1667 -0.1944 5.4978 -0.2000 -0.2333 5.4978 -0.2333 -0.2722 5.4978 -0.2667 -0.3111 5.4978 -0.3000 -0.3500 5.4978 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: 13 -9 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0793 -0.0378 5.8502 -0.1572 -0.0787 5.8100 -0.2334 -0.1229 5.7697 -0.3079 -0.1701 5.7295 -0.3805 -0.2204 5.6892 -0.4512 -0.2736 5.6490 -0.5197 -0.3296 5.6088 -0.5860 -0.3885 5.5685 -0.6500 -0.4500 5.5283 -primID: 3 -startangle_c: 15 -endpose_c: 14 -3 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0741 -0.0305 5.9058 -0.1493 -0.0582 5.9530 -0.2256 -0.0824 6.0002 -0.3031 -0.1030 6.0473 -0.3814 -0.1199 6.0945 -0.4604 -0.1330 6.1417 -0.5400 -0.1424 6.1888 -0.6199 -0.1481 6.2360 -0.7000 -0.1500 6.2832 -primID: 4 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 5 -startangle_c: 15 -endpose_c: 6 -1 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0320 -0.0105 5.9281 -0.0644 -0.0197 5.9658 -0.0973 -0.0278 6.0034 -0.1304 -0.0346 6.0410 -0.1639 -0.0402 6.0787 -0.1977 -0.0446 6.1163 -0.2316 -0.0476 6.1540 -0.2658 -0.0495 6.1916 -0.3000 -0.0500 6.2292 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 7 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 -primID: 8 -startangle_c: 15 -endpose_c: 7 -2 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0389 -0.0111 5.8905 -0.0778 -0.0222 5.8905 -0.1167 -0.0333 5.8905 -0.1556 -0.0444 5.8905 -0.1944 -0.0556 5.8905 -0.2333 -0.0667 5.8905 -0.2722 -0.0778 5.8905 -0.3111 -0.0889 5.8905 -0.3500 -0.1000 5.8905 -primID: 9 -startangle_c: 15 -endpose_c: 5 -4 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0278 -0.0222 5.8905 -0.0556 -0.0444 5.8905 -0.0833 -0.0667 5.8905 -0.1111 -0.0889 5.8905 -0.1389 -0.1111 5.8905 -0.1667 -0.1333 5.8905 -0.1944 -0.1556 5.8905 -0.2222 -0.1778 5.8905 -0.2500 -0.2000 5.8905 diff --git a/mir_navigation/mprim/unicycle_highcost_10cm.mprim b/mir_navigation/mprim/unicycle_highcost_10cm.mprim deleted file mode 100644 index de16976e..00000000 --- a/mir_navigation/mprim/unicycle_highcost_10cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.100000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0556 0.0000 0.0000 -0.0667 0.0000 0.0000 -0.0778 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1000 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 4 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0556 0.0000 0.0000 --0.0667 0.0000 0.0000 --0.0778 0.0000 0.0000 --0.0889 0.0000 0.0000 --0.1000 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 4 1 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0048 0.0349 -0.0903 0.0111 0.0699 -0.1353 0.0191 0.1048 -0.1802 0.0286 0.1398 -0.2248 0.0398 0.1747 -0.2692 0.0525 0.2097 -0.3132 0.0668 0.2446 -0.3568 0.0826 0.2796 -0.4000 0.1000 0.3145 -primID: 4 -startangle_c: 0 -endpose_c: 4 -1 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0048 -0.0349 -0.0903 -0.0111 -0.0699 -0.1353 -0.0191 -0.1048 -0.1802 -0.0286 -0.1398 -0.2248 -0.0398 -0.1747 -0.2692 -0.0525 -0.2097 -0.3132 -0.0668 -0.2446 -0.3568 -0.0826 -0.2796 -0.4000 -0.1000 -0.3145 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0222 0.0111 0.3927 -0.0444 0.0222 0.3927 -0.0667 0.0333 0.3927 -0.0889 0.0444 0.3927 -0.1111 0.0556 0.3927 -0.1333 0.0667 0.3927 -0.1556 0.0778 0.3927 -0.1778 0.0889 0.3927 -0.2000 0.1000 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0667 0.0333 0.3927 -0.1333 0.0667 0.3927 -0.2000 0.1000 0.3927 -0.2667 0.1333 0.3927 -0.3333 0.1667 0.3927 -0.4000 0.2000 0.3927 -0.4667 0.2333 0.3927 -0.5333 0.2667 0.3927 -0.6000 0.3000 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0222 -0.0111 0.3927 --0.0444 -0.0222 0.3927 --0.0667 -0.0333 0.3927 --0.0889 -0.0444 0.3927 --0.1111 -0.0556 0.3927 --0.1333 -0.0667 0.3927 --0.1556 -0.0778 0.3927 --0.1778 -0.0889 0.3927 --0.2000 -0.1000 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 3 2 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0369 0.0162 0.4345 -0.0731 0.0339 0.4783 -0.1085 0.0533 0.5222 -0.1430 0.0741 0.5661 -0.1766 0.0965 0.6099 -0.2091 0.1203 0.6538 -0.2405 0.1455 0.6977 -0.2709 0.1721 0.7415 -0.3000 0.2000 0.7854 -primID: 4 -startangle_c: 1 -endpose_c: 4 1 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0427 0.0177 0.3927 -0.0854 0.0354 0.3927 -0.1283 0.0523 0.3477 -0.1722 0.0668 0.2898 -0.2168 0.0787 0.2318 -0.2621 0.0880 0.1739 -0.3078 0.0947 0.1159 -0.3538 0.0987 0.0580 -0.4000 0.1000 0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0111 0.0111 0.7854 -0.0222 0.0222 0.7854 -0.0333 0.0333 0.7854 -0.0444 0.0444 0.7854 -0.0556 0.0556 0.7854 -0.0667 0.0667 0.7854 -0.0778 0.0778 0.7854 -0.0889 0.0889 0.7854 -0.1000 0.1000 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 3 3 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0111 -0.0111 0.7854 --0.0222 -0.0222 0.7854 --0.0333 -0.0333 0.7854 --0.0444 -0.0444 0.7854 --0.0556 -0.0556 0.7854 --0.0667 -0.0667 0.7854 --0.0778 -0.0778 0.7854 --0.0889 -0.0889 0.7854 --0.1000 -0.1000 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 2 4 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0262 0.0411 0.8119 -0.0515 0.0831 0.8384 -0.0758 0.1260 0.8649 -0.0991 0.1697 0.8913 -0.1214 0.2142 0.9178 -0.1426 0.2595 0.9443 -0.1628 0.3056 0.9708 -0.1820 0.3525 0.9973 -0.2000 0.4000 1.0238 -primID: 4 -startangle_c: 2 -endpose_c: 4 2 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0411 0.0262 0.7589 -0.0831 0.0515 0.7324 -0.1260 0.0758 0.7059 -0.1697 0.0991 0.6795 -0.2142 0.1214 0.6530 -0.2595 0.1426 0.6265 -0.3056 0.1628 0.6000 -0.3525 0.1820 0.5735 -0.4000 0.2000 0.5470 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0111 0.0222 1.1781 -0.0222 0.0444 1.1781 -0.0333 0.0667 1.1781 -0.0444 0.0889 1.1781 -0.0556 0.1111 1.1781 -0.0667 0.1333 1.1781 -0.0778 0.1556 1.1781 -0.0889 0.1778 1.1781 -0.1000 0.2000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0333 0.0667 1.1781 -0.0667 0.1333 1.1781 -0.1000 0.2000 1.1781 -0.1333 0.2667 1.1781 -0.1667 0.3333 1.1781 -0.2000 0.4000 1.1781 -0.2333 0.4667 1.1781 -0.2667 0.5333 1.1781 -0.3000 0.6000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0111 -0.0222 1.1781 --0.0222 -0.0444 1.1781 --0.0333 -0.0667 1.1781 --0.0444 -0.0889 1.1781 --0.0556 -0.1111 1.1781 --0.0667 -0.1333 1.1781 --0.0778 -0.1556 1.1781 --0.0889 -0.1778 1.1781 --0.1000 -0.2000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 2 3 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0162 0.0369 1.1363 -0.0339 0.0731 1.0925 -0.0533 0.1085 1.0486 -0.0741 0.1430 1.0047 -0.0965 0.1766 0.9609 -0.1203 0.2091 0.9170 -0.1455 0.2405 0.8731 -0.1721 0.2709 0.8293 -0.2000 0.3000 0.7854 -primID: 4 -startangle_c: 3 -endpose_c: 1 4 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0177 0.0427 1.1781 -0.0354 0.0854 1.1781 -0.0523 0.1283 1.2231 -0.0668 0.1722 1.2810 -0.0787 0.2168 1.3390 -0.0880 0.2621 1.3969 -0.0947 0.3078 1.4549 -0.0987 0.3538 1.5128 -0.1000 0.4000 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0556 1.5708 -0.0000 0.0667 1.5708 -0.0000 0.0778 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1000 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 4 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0556 1.5708 -0.0000 -0.0667 1.5708 -0.0000 -0.0778 1.5708 -0.0000 -0.0889 1.5708 -0.0000 -0.1000 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 4 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0048 0.0452 1.6057 --0.0111 0.0903 1.6407 --0.0191 0.1353 1.6756 --0.0286 0.1802 1.7106 --0.0398 0.2248 1.7455 --0.0525 0.2692 1.7805 --0.0668 0.3132 1.8154 --0.0826 0.3568 1.8503 --0.1000 0.4000 1.8853 -primID: 4 -startangle_c: 4 -endpose_c: 1 4 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0048 0.0452 1.5359 -0.0111 0.0903 1.5009 -0.0191 0.1353 1.4660 -0.0286 0.1802 1.4310 -0.0398 0.2248 1.3961 -0.0525 0.2692 1.3611 -0.0668 0.3132 1.3262 -0.0826 0.3568 1.2912 -0.1000 0.4000 1.2563 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0111 0.0222 1.9635 --0.0222 0.0444 1.9635 --0.0333 0.0667 1.9635 --0.0444 0.0889 1.9635 --0.0556 0.1111 1.9635 --0.0667 0.1333 1.9635 --0.0778 0.1556 1.9635 --0.0889 0.1778 1.9635 --0.1000 0.2000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0333 0.0667 1.9635 --0.0667 0.1333 1.9635 --0.1000 0.2000 1.9635 --0.1333 0.2667 1.9635 --0.1667 0.3333 1.9635 --0.2000 0.4000 1.9635 --0.2333 0.4667 1.9635 --0.2667 0.5333 1.9635 --0.3000 0.6000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0111 -0.0222 1.9635 -0.0222 -0.0444 1.9635 -0.0333 -0.0667 1.9635 -0.0444 -0.0889 1.9635 -0.0556 -0.1111 1.9635 -0.0667 -0.1333 1.9635 -0.0778 -0.1556 1.9635 -0.0889 -0.1778 1.9635 -0.1000 -0.2000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -2 3 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0162 0.0369 2.0053 --0.0339 0.0731 2.0491 --0.0533 0.1085 2.0930 --0.0741 0.1430 2.1369 --0.0965 0.1766 2.1807 --0.1203 0.2091 2.2246 --0.1455 0.2405 2.2685 --0.1721 0.2709 2.3123 --0.2000 0.3000 2.3562 -primID: 4 -startangle_c: 5 -endpose_c: -1 4 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0177 0.0427 1.9635 --0.0354 0.0854 1.9635 --0.0523 0.1283 1.9185 --0.0668 0.1722 1.8606 --0.0787 0.2168 1.8026 --0.0880 0.2621 1.7447 --0.0947 0.3078 1.6867 --0.0987 0.3538 1.6287 --0.1000 0.4000 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0111 0.0111 2.3562 --0.0222 0.0222 2.3562 --0.0333 0.0333 2.3562 --0.0444 0.0444 2.3562 --0.0556 0.0556 2.3562 --0.0667 0.0667 2.3562 --0.0778 0.0778 2.3562 --0.0889 0.0889 2.3562 --0.1000 0.1000 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -3 3 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0111 -0.0111 2.3562 -0.0222 -0.0222 2.3562 -0.0333 -0.0333 2.3562 -0.0444 -0.0444 2.3562 -0.0556 -0.0556 2.3562 -0.0667 -0.0667 2.3562 -0.0778 -0.0778 2.3562 -0.0889 -0.0889 2.3562 -0.1000 -0.1000 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -4 2 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0411 0.0262 2.3827 --0.0831 0.0515 2.4092 --0.1260 0.0758 2.4357 --0.1697 0.0991 2.4621 --0.2142 0.1214 2.4886 --0.2595 0.1426 2.5151 --0.3056 0.1628 2.5416 --0.3525 0.1820 2.5681 --0.4000 0.2000 2.5946 -primID: 4 -startangle_c: 6 -endpose_c: -2 4 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0262 0.0411 2.3297 --0.0515 0.0831 2.3032 --0.0758 0.1260 2.2767 --0.0991 0.1697 2.2502 --0.1214 0.2142 2.2238 --0.1426 0.2595 2.1973 --0.1628 0.3056 2.1708 --0.1820 0.3525 2.1443 --0.2000 0.4000 2.1178 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0222 0.0111 2.7489 --0.0444 0.0222 2.7489 --0.0667 0.0333 2.7489 --0.0889 0.0444 2.7489 --0.1111 0.0556 2.7489 --0.1333 0.0667 2.7489 --0.1556 0.0778 2.7489 --0.1778 0.0889 2.7489 --0.2000 0.1000 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0667 0.0333 2.7489 --0.1333 0.0667 2.7489 --0.2000 0.1000 2.7489 --0.2667 0.1333 2.7489 --0.3333 0.1667 2.7489 --0.4000 0.2000 2.7489 --0.4667 0.2333 2.7489 --0.5333 0.2667 2.7489 --0.6000 0.3000 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0222 -0.0111 2.7489 -0.0444 -0.0222 2.7489 -0.0667 -0.0333 2.7489 -0.0889 -0.0444 2.7489 -0.1111 -0.0556 2.7489 -0.1333 -0.0667 2.7489 -0.1556 -0.0778 2.7489 -0.1778 -0.0889 2.7489 -0.2000 -0.1000 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -3 2 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0369 0.0162 2.7071 --0.0731 0.0339 2.6633 --0.1085 0.0533 2.6194 --0.1430 0.0741 2.5755 --0.1766 0.0965 2.5317 --0.2091 0.1203 2.4878 --0.2405 0.1455 2.4439 --0.2709 0.1721 2.4001 --0.3000 0.2000 2.3562 -primID: 4 -startangle_c: 7 -endpose_c: -4 1 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0427 0.0177 2.7489 --0.0854 0.0354 2.7489 --0.1283 0.0523 2.7939 --0.1722 0.0668 2.8518 --0.2168 0.0787 2.9098 --0.2621 0.0880 2.9677 --0.3078 0.0947 3.0257 --0.3538 0.0987 3.0836 --0.4000 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0556 0.0000 3.1416 --0.0667 0.0000 3.1416 --0.0778 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1000 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -4 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0556 0.0000 3.1416 -0.0667 0.0000 3.1416 -0.0778 0.0000 3.1416 -0.0889 0.0000 3.1416 -0.1000 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -4 -1 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 -0.0048 3.1765 --0.0903 -0.0111 3.2115 --0.1353 -0.0191 3.2464 --0.1802 -0.0286 3.2814 --0.2248 -0.0398 3.3163 --0.2692 -0.0525 3.3513 --0.3132 -0.0668 3.3862 --0.3568 -0.0826 3.4211 --0.4000 -0.1000 3.4561 -primID: 4 -startangle_c: 8 -endpose_c: -4 1 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0048 3.1066 --0.0903 0.0111 3.0717 --0.1353 0.0191 3.0368 --0.1802 0.0286 3.0018 --0.2248 0.0398 2.9669 --0.2692 0.0525 2.9319 --0.3132 0.0668 2.8970 --0.3568 0.0826 2.8620 --0.4000 0.1000 2.8271 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0222 -0.0111 3.5343 --0.0444 -0.0222 3.5343 --0.0667 -0.0333 3.5343 --0.0889 -0.0444 3.5343 --0.1111 -0.0556 3.5343 --0.1333 -0.0667 3.5343 --0.1556 -0.0778 3.5343 --0.1778 -0.0889 3.5343 --0.2000 -0.1000 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0667 -0.0333 3.5343 --0.1333 -0.0667 3.5343 --0.2000 -0.1000 3.5343 --0.2667 -0.1333 3.5343 --0.3333 -0.1667 3.5343 --0.4000 -0.2000 3.5343 --0.4667 -0.2333 3.5343 --0.5333 -0.2667 3.5343 --0.6000 -0.3000 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0222 0.0111 3.5343 -0.0444 0.0222 3.5343 -0.0667 0.0333 3.5343 -0.0889 0.0444 3.5343 -0.1111 0.0556 3.5343 -0.1333 0.0667 3.5343 -0.1556 0.0778 3.5343 -0.1778 0.0889 3.5343 -0.2000 0.1000 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -3 -2 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0369 -0.0162 3.5761 --0.0731 -0.0339 3.6199 --0.1085 -0.0533 3.6638 --0.1430 -0.0741 3.7077 --0.1766 -0.0965 3.7515 --0.2091 -0.1203 3.7954 --0.2405 -0.1455 3.8393 --0.2709 -0.1721 3.8831 --0.3000 -0.2000 3.9270 -primID: 4 -startangle_c: 9 -endpose_c: -4 -1 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0427 -0.0177 3.5343 --0.0854 -0.0354 3.5343 --0.1283 -0.0523 3.4893 --0.1722 -0.0668 3.4313 --0.2168 -0.0787 3.3734 --0.2621 -0.0880 3.3154 --0.3078 -0.0947 3.2575 --0.3538 -0.0987 3.1995 --0.4000 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0111 -0.0111 3.9270 --0.0222 -0.0222 3.9270 --0.0333 -0.0333 3.9270 --0.0444 -0.0444 3.9270 --0.0556 -0.0556 3.9270 --0.0667 -0.0667 3.9270 --0.0778 -0.0778 3.9270 --0.0889 -0.0889 3.9270 --0.1000 -0.1000 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -3 -3 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0111 0.0111 3.9270 -0.0222 0.0222 3.9270 -0.0333 0.0333 3.9270 -0.0444 0.0444 3.9270 -0.0556 0.0556 3.9270 -0.0667 0.0667 3.9270 -0.0778 0.0778 3.9270 -0.0889 0.0889 3.9270 -0.1000 0.1000 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -2 -4 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0262 -0.0411 3.9535 --0.0515 -0.0831 3.9800 --0.0758 -0.1260 4.0064 --0.0991 -0.1697 4.0329 --0.1214 -0.2142 4.0594 --0.1426 -0.2595 4.0859 --0.1628 -0.3056 4.1124 --0.1820 -0.3525 4.1389 --0.2000 -0.4000 4.1654 -primID: 4 -startangle_c: 10 -endpose_c: -4 -2 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0411 -0.0262 3.9005 --0.0831 -0.0515 3.8740 --0.1260 -0.0758 3.8475 --0.1697 -0.0991 3.8210 --0.2142 -0.1214 3.7946 --0.2595 -0.1426 3.7681 --0.3056 -0.1628 3.7416 --0.3525 -0.1820 3.7151 --0.4000 -0.2000 3.6886 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0111 -0.0222 4.3197 --0.0222 -0.0444 4.3197 --0.0333 -0.0667 4.3197 --0.0444 -0.0889 4.3197 --0.0556 -0.1111 4.3197 --0.0667 -0.1333 4.3197 --0.0778 -0.1556 4.3197 --0.0889 -0.1778 4.3197 --0.1000 -0.2000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0333 -0.0667 4.3197 --0.0667 -0.1333 4.3197 --0.1000 -0.2000 4.3197 --0.1333 -0.2667 4.3197 --0.1667 -0.3333 4.3197 --0.2000 -0.4000 4.3197 --0.2333 -0.4667 4.3197 --0.2667 -0.5333 4.3197 --0.3000 -0.6000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0111 0.0222 4.3197 -0.0222 0.0444 4.3197 -0.0333 0.0667 4.3197 -0.0444 0.0889 4.3197 -0.0556 0.1111 4.3197 -0.0667 0.1333 4.3197 -0.0778 0.1556 4.3197 -0.0889 0.1778 4.3197 -0.1000 0.2000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -2 -3 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0162 -0.0369 4.2779 --0.0339 -0.0731 4.2341 --0.0533 -0.1085 4.1902 --0.0741 -0.1430 4.1463 --0.0965 -0.1766 4.1025 --0.1203 -0.2091 4.0586 --0.1455 -0.2405 4.0147 --0.1721 -0.2709 3.9709 --0.2000 -0.3000 3.9270 -primID: 4 -startangle_c: 11 -endpose_c: -1 -4 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0177 -0.0427 4.3197 --0.0354 -0.0854 4.3197 --0.0523 -0.1283 4.3647 --0.0668 -0.1722 4.4226 --0.0787 -0.2168 4.4806 --0.0880 -0.2621 4.5385 --0.0947 -0.3078 4.5965 --0.0987 -0.3538 4.6544 --0.1000 -0.4000 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0556 4.7124 -0.0000 -0.0667 4.7124 -0.0000 -0.0778 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1000 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -4 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0556 4.7124 -0.0000 0.0667 4.7124 -0.0000 0.0778 4.7124 -0.0000 0.0889 4.7124 -0.0000 0.1000 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -4 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0048 -0.0452 4.7473 -0.0111 -0.0903 4.7823 -0.0191 -0.1353 4.8172 -0.0286 -0.1802 4.8522 -0.0398 -0.2248 4.8871 -0.0525 -0.2692 4.9221 -0.0668 -0.3132 4.9570 -0.0826 -0.3568 4.9919 -0.1000 -0.4000 5.0269 -primID: 4 -startangle_c: 12 -endpose_c: -1 -4 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0048 -0.0452 4.6774 --0.0111 -0.0903 4.6425 --0.0191 -0.1353 4.6076 --0.0286 -0.1802 4.5726 --0.0398 -0.2248 4.5377 --0.0525 -0.2692 4.5027 --0.0668 -0.3132 4.4678 --0.0826 -0.3568 4.4328 --0.1000 -0.4000 4.3979 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0111 -0.0222 5.1051 -0.0222 -0.0444 5.1051 -0.0333 -0.0667 5.1051 -0.0444 -0.0889 5.1051 -0.0556 -0.1111 5.1051 -0.0667 -0.1333 5.1051 -0.0778 -0.1556 5.1051 -0.0889 -0.1778 5.1051 -0.1000 -0.2000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0333 -0.0667 5.1051 -0.0667 -0.1333 5.1051 -0.1000 -0.2000 5.1051 -0.1333 -0.2667 5.1051 -0.1667 -0.3333 5.1051 -0.2000 -0.4000 5.1051 -0.2333 -0.4667 5.1051 -0.2667 -0.5333 5.1051 -0.3000 -0.6000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0111 0.0222 5.1051 --0.0222 0.0444 5.1051 --0.0333 0.0667 5.1051 --0.0444 0.0889 5.1051 --0.0556 0.1111 5.1051 --0.0667 0.1333 5.1051 --0.0778 0.1556 5.1051 --0.0889 0.1778 5.1051 --0.1000 0.2000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 2 -3 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0162 -0.0369 5.1469 -0.0339 -0.0731 5.1907 -0.0533 -0.1085 5.2346 -0.0741 -0.1430 5.2785 -0.0965 -0.1766 5.3223 -0.1203 -0.2091 5.3662 -0.1455 -0.2405 5.4101 -0.1721 -0.2709 5.4539 -0.2000 -0.3000 5.4978 -primID: 4 -startangle_c: 13 -endpose_c: 1 -4 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0177 -0.0427 5.1051 -0.0354 -0.0854 5.1051 -0.0523 -0.1283 5.0601 -0.0668 -0.1722 5.0021 -0.0787 -0.2168 4.9442 -0.0880 -0.2621 4.8862 -0.0947 -0.3078 4.8283 -0.0987 -0.3538 4.7703 -0.1000 -0.4000 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0111 -0.0111 5.4978 -0.0222 -0.0222 5.4978 -0.0333 -0.0333 5.4978 -0.0444 -0.0444 5.4978 -0.0556 -0.0556 5.4978 -0.0667 -0.0667 5.4978 -0.0778 -0.0778 5.4978 -0.0889 -0.0889 5.4978 -0.1000 -0.1000 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 3 -3 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0111 0.0111 5.4978 --0.0222 0.0222 5.4978 --0.0333 0.0333 5.4978 --0.0444 0.0444 5.4978 --0.0556 0.0556 5.4978 --0.0667 0.0667 5.4978 --0.0778 0.0778 5.4978 --0.0889 0.0889 5.4978 --0.1000 0.1000 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 4 -2 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0411 -0.0262 5.5243 -0.0831 -0.0515 5.5508 -0.1260 -0.0758 5.5772 -0.1697 -0.0991 5.6037 -0.2142 -0.1214 5.6302 -0.2595 -0.1426 5.6567 -0.3056 -0.1628 5.6832 -0.3525 -0.1820 5.7097 -0.4000 -0.2000 5.7362 -primID: 4 -startangle_c: 14 -endpose_c: 2 -4 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0262 -0.0411 5.4713 -0.0515 -0.0831 5.4448 -0.0758 -0.1260 5.4183 -0.0991 -0.1697 5.3918 -0.1214 -0.2142 5.3654 -0.1426 -0.2595 5.3389 -0.1628 -0.3056 5.3124 -0.1820 -0.3525 5.2859 -0.2000 -0.4000 5.2594 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0222 -0.0111 5.8905 -0.0444 -0.0222 5.8905 -0.0667 -0.0333 5.8905 -0.0889 -0.0444 5.8905 -0.1111 -0.0556 5.8905 -0.1333 -0.0667 5.8905 -0.1556 -0.0778 5.8905 -0.1778 -0.0889 5.8905 -0.2000 -0.1000 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0667 -0.0333 5.8905 -0.1333 -0.0667 5.8905 -0.2000 -0.1000 5.8905 -0.2667 -0.1333 5.8905 -0.3333 -0.1667 5.8905 -0.4000 -0.2000 5.8905 -0.4667 -0.2333 5.8905 -0.5333 -0.2667 5.8905 -0.6000 -0.3000 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0222 0.0111 5.8905 --0.0444 0.0222 5.8905 --0.0667 0.0333 5.8905 --0.0889 0.0444 5.8905 --0.1111 0.0556 5.8905 --0.1333 0.0667 5.8905 --0.1556 0.0778 5.8905 --0.1778 0.0889 5.8905 --0.2000 0.1000 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 3 -2 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0369 -0.0162 5.8487 -0.0731 -0.0339 5.8049 -0.1085 -0.0533 5.7610 -0.1430 -0.0741 5.7171 -0.1766 -0.0965 5.6733 -0.2091 -0.1203 5.6294 -0.2405 -0.1455 5.5855 -0.2709 -0.1721 5.5417 -0.3000 -0.2000 5.4978 -primID: 4 -startangle_c: 15 -endpose_c: 4 -1 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0427 -0.0177 5.8905 -0.0854 -0.0354 5.8905 -0.1283 -0.0523 5.9355 -0.1722 -0.0668 5.9934 -0.2168 -0.0787 6.0514 -0.2621 -0.0880 6.1093 -0.3078 -0.0947 6.1673 -0.3538 -0.0987 6.2252 -0.4000 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/mir_navigation/mprim/unicycle_highcost_1cm.mprim b/mir_navigation/mprim/unicycle_highcost_1cm.mprim deleted file mode 100644 index aa9670de..00000000 --- a/mir_navigation/mprim/unicycle_highcost_1cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.010000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0011 0.0000 0.0000 -0.0022 0.0000 0.0000 -0.0033 0.0000 0.0000 -0.0044 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0067 0.0000 0.0000 -0.0078 0.0000 0.0000 -0.0089 0.0000 0.0000 -0.0100 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 20 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0667 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1111 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1556 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0011 0.0000 0.0000 --0.0022 0.0000 0.0000 --0.0033 0.0000 0.0000 --0.0044 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0067 0.0000 0.0000 --0.0078 0.0000 0.0000 --0.0089 0.0000 0.0000 --0.0100 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 40 5 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 40 -5 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0022 0.0011 0.3927 -0.0044 0.0022 0.3927 -0.0067 0.0033 0.3927 -0.0089 0.0044 0.3927 -0.0111 0.0056 0.3927 -0.0133 0.0067 0.3927 -0.0156 0.0078 0.3927 -0.0178 0.0089 0.3927 -0.0200 0.0100 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 20 10 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0222 0.0111 0.3927 -0.0444 0.0222 0.3927 -0.0667 0.0333 0.3927 -0.0889 0.0444 0.3927 -0.1111 0.0556 0.3927 -0.1333 0.0667 0.3927 -0.1556 0.0778 0.3927 -0.1778 0.0889 0.3927 -0.2000 0.1000 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0022 -0.0011 0.3927 --0.0044 -0.0022 0.3927 --0.0067 -0.0033 0.3927 --0.0089 -0.0044 0.3927 --0.0111 -0.0056 0.3927 --0.0133 -0.0067 0.3927 --0.0156 -0.0078 0.3927 --0.0178 -0.0089 0.3927 --0.0200 -0.0100 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 25 20 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 35 10 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0011 0.0011 0.7854 -0.0022 0.0022 0.7854 -0.0033 0.0033 0.7854 -0.0044 0.0044 0.7854 -0.0056 0.0056 0.7854 -0.0067 0.0067 0.7854 -0.0078 0.0078 0.7854 -0.0089 0.0089 0.7854 -0.0100 0.0100 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 10 10 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0111 0.0111 0.7854 -0.0222 0.0222 0.7854 -0.0333 0.0333 0.7854 -0.0444 0.0444 0.7854 -0.0556 0.0556 0.7854 -0.0667 0.0667 0.7854 -0.0778 0.0778 0.7854 -0.0889 0.0889 0.7854 -0.1000 0.1000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0011 -0.0011 0.7854 --0.0022 -0.0022 0.7854 --0.0033 -0.0033 0.7854 --0.0044 -0.0044 0.7854 --0.0056 -0.0056 0.7854 --0.0067 -0.0067 0.7854 --0.0078 -0.0078 0.7854 --0.0089 -0.0089 0.7854 --0.0100 -0.0100 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 25 35 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 35 25 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0011 0.0022 1.1781 -0.0022 0.0044 1.1781 -0.0033 0.0067 1.1781 -0.0044 0.0089 1.1781 -0.0056 0.0111 1.1781 -0.0067 0.0133 1.1781 -0.0078 0.0156 1.1781 -0.0089 0.0178 1.1781 -0.0100 0.0200 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 10 20 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0111 0.0222 1.1781 -0.0222 0.0444 1.1781 -0.0333 0.0667 1.1781 -0.0444 0.0889 1.1781 -0.0556 0.1111 1.1781 -0.0667 0.1333 1.1781 -0.0778 0.1556 1.1781 -0.0889 0.1778 1.1781 -0.1000 0.2000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0011 -0.0022 1.1781 --0.0022 -0.0044 1.1781 --0.0033 -0.0067 1.1781 --0.0044 -0.0089 1.1781 --0.0056 -0.0111 1.1781 --0.0067 -0.0133 1.1781 --0.0078 -0.0156 1.1781 --0.0089 -0.0178 1.1781 --0.0100 -0.0200 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 20 25 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 10 35 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0011 1.5708 -0.0000 0.0022 1.5708 -0.0000 0.0033 1.5708 -0.0000 0.0044 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0067 1.5708 -0.0000 0.0078 1.5708 -0.0000 0.0089 1.5708 -0.0000 0.0100 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 20 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0667 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1111 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1556 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0011 1.5708 -0.0000 -0.0022 1.5708 -0.0000 -0.0033 1.5708 -0.0000 -0.0044 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0067 1.5708 -0.0000 -0.0078 1.5708 -0.0000 -0.0089 1.5708 -0.0000 -0.0100 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -5 40 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 5 40 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0011 0.0022 1.9635 --0.0022 0.0044 1.9635 --0.0033 0.0067 1.9635 --0.0044 0.0089 1.9635 --0.0056 0.0111 1.9635 --0.0067 0.0133 1.9635 --0.0078 0.0156 1.9635 --0.0089 0.0178 1.9635 --0.0100 0.0200 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -10 20 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0111 0.0222 1.9635 --0.0222 0.0444 1.9635 --0.0333 0.0667 1.9635 --0.0444 0.0889 1.9635 --0.0556 0.1111 1.9635 --0.0667 0.1333 1.9635 --0.0778 0.1556 1.9635 --0.0889 0.1778 1.9635 --0.1000 0.2000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0011 -0.0022 1.9635 -0.0022 -0.0044 1.9635 -0.0033 -0.0067 1.9635 -0.0044 -0.0089 1.9635 -0.0056 -0.0111 1.9635 -0.0067 -0.0133 1.9635 -0.0078 -0.0156 1.9635 -0.0089 -0.0178 1.9635 -0.0100 -0.0200 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -20 25 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -10 35 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0011 0.0011 2.3562 --0.0022 0.0022 2.3562 --0.0033 0.0033 2.3562 --0.0044 0.0044 2.3562 --0.0056 0.0056 2.3562 --0.0067 0.0067 2.3562 --0.0078 0.0078 2.3562 --0.0089 0.0089 2.3562 --0.0100 0.0100 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -10 10 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0111 0.0111 2.3562 --0.0222 0.0222 2.3562 --0.0333 0.0333 2.3562 --0.0444 0.0444 2.3562 --0.0556 0.0556 2.3562 --0.0667 0.0667 2.3562 --0.0778 0.0778 2.3562 --0.0889 0.0889 2.3562 --0.1000 0.1000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0011 -0.0011 2.3562 -0.0022 -0.0022 2.3562 -0.0033 -0.0033 2.3562 -0.0044 -0.0044 2.3562 -0.0056 -0.0056 2.3562 -0.0067 -0.0067 2.3562 -0.0078 -0.0078 2.3562 -0.0089 -0.0089 2.3562 -0.0100 -0.0100 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -35 25 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -25 35 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0022 0.0011 2.7489 --0.0044 0.0022 2.7489 --0.0067 0.0033 2.7489 --0.0089 0.0044 2.7489 --0.0111 0.0056 2.7489 --0.0133 0.0067 2.7489 --0.0156 0.0078 2.7489 --0.0178 0.0089 2.7489 --0.0200 0.0100 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -20 10 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0222 0.0111 2.7489 --0.0444 0.0222 2.7489 --0.0667 0.0333 2.7489 --0.0889 0.0444 2.7489 --0.1111 0.0556 2.7489 --0.1333 0.0667 2.7489 --0.1556 0.0778 2.7489 --0.1778 0.0889 2.7489 --0.2000 0.1000 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0022 -0.0011 2.7489 -0.0044 -0.0022 2.7489 -0.0067 -0.0033 2.7489 -0.0089 -0.0044 2.7489 -0.0111 -0.0056 2.7489 -0.0133 -0.0067 2.7489 -0.0156 -0.0078 2.7489 -0.0178 -0.0089 2.7489 -0.0200 -0.0100 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -25 20 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -35 10 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0011 0.0000 3.1416 --0.0022 0.0000 3.1416 --0.0033 0.0000 3.1416 --0.0044 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0067 0.0000 3.1416 --0.0078 0.0000 3.1416 --0.0089 0.0000 3.1416 --0.0100 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -20 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0667 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1111 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1556 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0011 0.0000 3.1416 -0.0022 0.0000 3.1416 -0.0033 0.0000 3.1416 -0.0044 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0067 0.0000 3.1416 -0.0078 0.0000 3.1416 -0.0089 0.0000 3.1416 -0.0100 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -40 -5 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -40 5 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0022 -0.0011 3.5343 --0.0044 -0.0022 3.5343 --0.0067 -0.0033 3.5343 --0.0089 -0.0044 3.5343 --0.0111 -0.0056 3.5343 --0.0133 -0.0067 3.5343 --0.0156 -0.0078 3.5343 --0.0178 -0.0089 3.5343 --0.0200 -0.0100 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -20 -10 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0222 -0.0111 3.5343 --0.0444 -0.0222 3.5343 --0.0667 -0.0333 3.5343 --0.0889 -0.0444 3.5343 --0.1111 -0.0556 3.5343 --0.1333 -0.0667 3.5343 --0.1556 -0.0778 3.5343 --0.1778 -0.0889 3.5343 --0.2000 -0.1000 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0022 0.0011 3.5343 -0.0044 0.0022 3.5343 -0.0067 0.0033 3.5343 -0.0089 0.0044 3.5343 -0.0111 0.0056 3.5343 -0.0133 0.0067 3.5343 -0.0156 0.0078 3.5343 -0.0178 0.0089 3.5343 -0.0200 0.0100 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -25 -20 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -35 -10 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0011 -0.0011 3.9270 --0.0022 -0.0022 3.9270 --0.0033 -0.0033 3.9270 --0.0044 -0.0044 3.9270 --0.0056 -0.0056 3.9270 --0.0067 -0.0067 3.9270 --0.0078 -0.0078 3.9270 --0.0089 -0.0089 3.9270 --0.0100 -0.0100 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -10 -10 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0111 -0.0111 3.9270 --0.0222 -0.0222 3.9270 --0.0333 -0.0333 3.9270 --0.0444 -0.0444 3.9270 --0.0556 -0.0556 3.9270 --0.0667 -0.0667 3.9270 --0.0778 -0.0778 3.9270 --0.0889 -0.0889 3.9270 --0.1000 -0.1000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0011 0.0011 3.9270 -0.0022 0.0022 3.9270 -0.0033 0.0033 3.9270 -0.0044 0.0044 3.9270 -0.0056 0.0056 3.9270 -0.0067 0.0067 3.9270 -0.0078 0.0078 3.9270 -0.0089 0.0089 3.9270 -0.0100 0.0100 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -25 -35 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -35 -25 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0011 -0.0022 4.3197 --0.0022 -0.0044 4.3197 --0.0033 -0.0067 4.3197 --0.0044 -0.0089 4.3197 --0.0056 -0.0111 4.3197 --0.0067 -0.0133 4.3197 --0.0078 -0.0156 4.3197 --0.0089 -0.0178 4.3197 --0.0100 -0.0200 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -10 -20 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0111 -0.0222 4.3197 --0.0222 -0.0444 4.3197 --0.0333 -0.0667 4.3197 --0.0444 -0.0889 4.3197 --0.0556 -0.1111 4.3197 --0.0667 -0.1333 4.3197 --0.0778 -0.1556 4.3197 --0.0889 -0.1778 4.3197 --0.1000 -0.2000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0011 0.0022 4.3197 -0.0022 0.0044 4.3197 -0.0033 0.0067 4.3197 -0.0044 0.0089 4.3197 -0.0056 0.0111 4.3197 -0.0067 0.0133 4.3197 -0.0078 0.0156 4.3197 -0.0089 0.0178 4.3197 -0.0100 0.0200 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -20 -25 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -10 -35 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0011 4.7124 -0.0000 -0.0022 4.7124 -0.0000 -0.0033 4.7124 -0.0000 -0.0044 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0067 4.7124 -0.0000 -0.0078 4.7124 -0.0000 -0.0089 4.7124 -0.0000 -0.0100 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -20 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0667 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1111 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1556 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0011 4.7124 -0.0000 0.0022 4.7124 -0.0000 0.0033 4.7124 -0.0000 0.0044 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0067 4.7124 -0.0000 0.0078 4.7124 -0.0000 0.0089 4.7124 -0.0000 0.0100 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 5 -40 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -5 -40 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0011 -0.0022 5.1051 -0.0022 -0.0044 5.1051 -0.0033 -0.0067 5.1051 -0.0044 -0.0089 5.1051 -0.0056 -0.0111 5.1051 -0.0067 -0.0133 5.1051 -0.0078 -0.0156 5.1051 -0.0089 -0.0178 5.1051 -0.0100 -0.0200 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 10 -20 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0111 -0.0222 5.1051 -0.0222 -0.0444 5.1051 -0.0333 -0.0667 5.1051 -0.0444 -0.0889 5.1051 -0.0556 -0.1111 5.1051 -0.0667 -0.1333 5.1051 -0.0778 -0.1556 5.1051 -0.0889 -0.1778 5.1051 -0.1000 -0.2000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0011 0.0022 5.1051 --0.0022 0.0044 5.1051 --0.0033 0.0067 5.1051 --0.0044 0.0089 5.1051 --0.0056 0.0111 5.1051 --0.0067 0.0133 5.1051 --0.0078 0.0156 5.1051 --0.0089 0.0178 5.1051 --0.0100 0.0200 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 20 -25 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 10 -35 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0011 -0.0011 5.4978 -0.0022 -0.0022 5.4978 -0.0033 -0.0033 5.4978 -0.0044 -0.0044 5.4978 -0.0056 -0.0056 5.4978 -0.0067 -0.0067 5.4978 -0.0078 -0.0078 5.4978 -0.0089 -0.0089 5.4978 -0.0100 -0.0100 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 10 -10 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0111 -0.0111 5.4978 -0.0222 -0.0222 5.4978 -0.0333 -0.0333 5.4978 -0.0444 -0.0444 5.4978 -0.0556 -0.0556 5.4978 -0.0667 -0.0667 5.4978 -0.0778 -0.0778 5.4978 -0.0889 -0.0889 5.4978 -0.1000 -0.1000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0011 0.0011 5.4978 --0.0022 0.0022 5.4978 --0.0033 0.0033 5.4978 --0.0044 0.0044 5.4978 --0.0056 0.0056 5.4978 --0.0067 0.0067 5.4978 --0.0078 0.0078 5.4978 --0.0089 0.0089 5.4978 --0.0100 0.0100 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 35 -25 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 25 -35 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0022 -0.0011 5.8905 -0.0044 -0.0022 5.8905 -0.0067 -0.0033 5.8905 -0.0089 -0.0044 5.8905 -0.0111 -0.0056 5.8905 -0.0133 -0.0067 5.8905 -0.0156 -0.0078 5.8905 -0.0178 -0.0089 5.8905 -0.0200 -0.0100 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 20 -10 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0222 -0.0111 5.8905 -0.0444 -0.0222 5.8905 -0.0667 -0.0333 5.8905 -0.0889 -0.0444 5.8905 -0.1111 -0.0556 5.8905 -0.1333 -0.0667 5.8905 -0.1556 -0.0778 5.8905 -0.1778 -0.0889 5.8905 -0.2000 -0.1000 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0022 0.0011 5.8905 --0.0044 0.0022 5.8905 --0.0067 0.0033 5.8905 --0.0089 0.0044 5.8905 --0.0111 0.0056 5.8905 --0.0133 0.0067 5.8905 --0.0156 0.0078 5.8905 --0.0178 0.0089 5.8905 --0.0200 0.0100 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 25 -20 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 35 -10 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/mir_navigation/mprim/unicycle_highcost_2_5cm.mprim b/mir_navigation/mprim/unicycle_highcost_2_5cm.mprim deleted file mode 100644 index 6bf962ec..00000000 --- a/mir_navigation/mprim/unicycle_highcost_2_5cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.025000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0028 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0083 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0139 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0194 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0250 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 12 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0667 0.0000 0.0000 -0.1000 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1667 0.0000 0.0000 -0.2000 0.0000 0.0000 -0.2333 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0028 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0083 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0139 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0194 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0250 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 16 2 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 16 -2 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0056 0.0028 0.3927 -0.0111 0.0056 0.3927 -0.0167 0.0083 0.3927 -0.0222 0.0111 0.3927 -0.0278 0.0139 0.3927 -0.0333 0.0167 0.3927 -0.0389 0.0194 0.3927 -0.0444 0.0222 0.3927 -0.0500 0.0250 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 12 6 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0056 -0.0028 0.3927 --0.0111 -0.0056 0.3927 --0.0167 -0.0083 0.3927 --0.0222 -0.0111 0.3927 --0.0278 -0.0139 0.3927 --0.0333 -0.0167 0.3927 --0.0389 -0.0194 0.3927 --0.0444 -0.0222 0.3927 --0.0500 -0.0250 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 10 8 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 14 4 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0028 0.0028 0.7854 -0.0056 0.0056 0.7854 -0.0083 0.0083 0.7854 -0.0111 0.0111 0.7854 -0.0139 0.0139 0.7854 -0.0167 0.0167 0.7854 -0.0194 0.0194 0.7854 -0.0222 0.0222 0.7854 -0.0250 0.0250 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 12 12 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0028 -0.0028 0.7854 --0.0056 -0.0056 0.7854 --0.0083 -0.0083 0.7854 --0.0111 -0.0111 0.7854 --0.0139 -0.0139 0.7854 --0.0167 -0.0167 0.7854 --0.0194 -0.0194 0.7854 --0.0222 -0.0222 0.7854 --0.0250 -0.0250 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 10 14 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 14 10 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0028 0.0056 1.1781 -0.0056 0.0111 1.1781 -0.0083 0.0167 1.1781 -0.0111 0.0222 1.1781 -0.0139 0.0278 1.1781 -0.0167 0.0333 1.1781 -0.0194 0.0389 1.1781 -0.0222 0.0444 1.1781 -0.0250 0.0500 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 6 12 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0028 -0.0056 1.1781 --0.0056 -0.0111 1.1781 --0.0083 -0.0167 1.1781 --0.0111 -0.0222 1.1781 --0.0139 -0.0278 1.1781 --0.0167 -0.0333 1.1781 --0.0194 -0.0389 1.1781 --0.0222 -0.0444 1.1781 --0.0250 -0.0500 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 8 10 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 4 14 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0028 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0083 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0139 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0194 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0250 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 12 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0667 1.5708 -0.0000 0.1000 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1667 1.5708 -0.0000 0.2000 1.5708 -0.0000 0.2333 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0028 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0083 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0139 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0194 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0250 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -2 16 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 2 16 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0028 0.0056 1.9635 --0.0056 0.0111 1.9635 --0.0083 0.0167 1.9635 --0.0111 0.0222 1.9635 --0.0139 0.0278 1.9635 --0.0167 0.0333 1.9635 --0.0194 0.0389 1.9635 --0.0222 0.0444 1.9635 --0.0250 0.0500 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -6 12 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0028 -0.0056 1.9635 -0.0056 -0.0111 1.9635 -0.0083 -0.0167 1.9635 -0.0111 -0.0222 1.9635 -0.0139 -0.0278 1.9635 -0.0167 -0.0333 1.9635 -0.0194 -0.0389 1.9635 -0.0222 -0.0444 1.9635 -0.0250 -0.0500 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -8 10 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -4 14 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0028 0.0028 2.3562 --0.0056 0.0056 2.3562 --0.0083 0.0083 2.3562 --0.0111 0.0111 2.3562 --0.0139 0.0139 2.3562 --0.0167 0.0167 2.3562 --0.0194 0.0194 2.3562 --0.0222 0.0222 2.3562 --0.0250 0.0250 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -12 12 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0028 -0.0028 2.3562 -0.0056 -0.0056 2.3562 -0.0083 -0.0083 2.3562 -0.0111 -0.0111 2.3562 -0.0139 -0.0139 2.3562 -0.0167 -0.0167 2.3562 -0.0194 -0.0194 2.3562 -0.0222 -0.0222 2.3562 -0.0250 -0.0250 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -14 10 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -10 14 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0056 0.0028 2.7489 --0.0111 0.0056 2.7489 --0.0167 0.0083 2.7489 --0.0222 0.0111 2.7489 --0.0278 0.0139 2.7489 --0.0333 0.0167 2.7489 --0.0389 0.0194 2.7489 --0.0444 0.0222 2.7489 --0.0500 0.0250 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -12 6 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0056 -0.0028 2.7489 -0.0111 -0.0056 2.7489 -0.0167 -0.0083 2.7489 -0.0222 -0.0111 2.7489 -0.0278 -0.0139 2.7489 -0.0333 -0.0167 2.7489 -0.0389 -0.0194 2.7489 -0.0444 -0.0222 2.7489 -0.0500 -0.0250 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -10 8 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -14 4 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0028 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0083 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0139 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0194 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0250 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -12 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0667 0.0000 3.1416 --0.1000 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1667 0.0000 3.1416 --0.2000 0.0000 3.1416 --0.2333 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0028 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0083 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0139 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0194 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0250 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -16 -2 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -16 2 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0056 -0.0028 3.5343 --0.0111 -0.0056 3.5343 --0.0167 -0.0083 3.5343 --0.0222 -0.0111 3.5343 --0.0278 -0.0139 3.5343 --0.0333 -0.0167 3.5343 --0.0389 -0.0194 3.5343 --0.0444 -0.0222 3.5343 --0.0500 -0.0250 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -12 -6 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0056 0.0028 3.5343 -0.0111 0.0056 3.5343 -0.0167 0.0083 3.5343 -0.0222 0.0111 3.5343 -0.0278 0.0139 3.5343 -0.0333 0.0167 3.5343 -0.0389 0.0194 3.5343 -0.0444 0.0222 3.5343 -0.0500 0.0250 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -10 -8 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -14 -4 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0028 -0.0028 3.9270 --0.0056 -0.0056 3.9270 --0.0083 -0.0083 3.9270 --0.0111 -0.0111 3.9270 --0.0139 -0.0139 3.9270 --0.0167 -0.0167 3.9270 --0.0194 -0.0194 3.9270 --0.0222 -0.0222 3.9270 --0.0250 -0.0250 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -12 -12 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0028 0.0028 3.9270 -0.0056 0.0056 3.9270 -0.0083 0.0083 3.9270 -0.0111 0.0111 3.9270 -0.0139 0.0139 3.9270 -0.0167 0.0167 3.9270 -0.0194 0.0194 3.9270 -0.0222 0.0222 3.9270 -0.0250 0.0250 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -10 -14 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -14 -10 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0028 -0.0056 4.3197 --0.0056 -0.0111 4.3197 --0.0083 -0.0167 4.3197 --0.0111 -0.0222 4.3197 --0.0139 -0.0278 4.3197 --0.0167 -0.0333 4.3197 --0.0194 -0.0389 4.3197 --0.0222 -0.0444 4.3197 --0.0250 -0.0500 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -6 -12 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0028 0.0056 4.3197 -0.0056 0.0111 4.3197 -0.0083 0.0167 4.3197 -0.0111 0.0222 4.3197 -0.0139 0.0278 4.3197 -0.0167 0.0333 4.3197 -0.0194 0.0389 4.3197 -0.0222 0.0444 4.3197 -0.0250 0.0500 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -8 -10 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -4 -14 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0028 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0083 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0139 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0194 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0250 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -12 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0667 4.7124 -0.0000 -0.1000 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1667 4.7124 -0.0000 -0.2000 4.7124 -0.0000 -0.2333 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0028 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0083 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0139 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0194 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0250 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 2 -16 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -2 -16 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0028 -0.0056 5.1051 -0.0056 -0.0111 5.1051 -0.0083 -0.0167 5.1051 -0.0111 -0.0222 5.1051 -0.0139 -0.0278 5.1051 -0.0167 -0.0333 5.1051 -0.0194 -0.0389 5.1051 -0.0222 -0.0444 5.1051 -0.0250 -0.0500 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 6 -12 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0028 0.0056 5.1051 --0.0056 0.0111 5.1051 --0.0083 0.0167 5.1051 --0.0111 0.0222 5.1051 --0.0139 0.0278 5.1051 --0.0167 0.0333 5.1051 --0.0194 0.0389 5.1051 --0.0222 0.0444 5.1051 --0.0250 0.0500 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 8 -10 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 4 -14 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0028 -0.0028 5.4978 -0.0056 -0.0056 5.4978 -0.0083 -0.0083 5.4978 -0.0111 -0.0111 5.4978 -0.0139 -0.0139 5.4978 -0.0167 -0.0167 5.4978 -0.0194 -0.0194 5.4978 -0.0222 -0.0222 5.4978 -0.0250 -0.0250 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 12 -12 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0028 0.0028 5.4978 --0.0056 0.0056 5.4978 --0.0083 0.0083 5.4978 --0.0111 0.0111 5.4978 --0.0139 0.0139 5.4978 --0.0167 0.0167 5.4978 --0.0194 0.0194 5.4978 --0.0222 0.0222 5.4978 --0.0250 0.0250 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 14 -10 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 10 -14 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0056 -0.0028 5.8905 -0.0111 -0.0056 5.8905 -0.0167 -0.0083 5.8905 -0.0222 -0.0111 5.8905 -0.0278 -0.0139 5.8905 -0.0333 -0.0167 5.8905 -0.0389 -0.0194 5.8905 -0.0444 -0.0222 5.8905 -0.0500 -0.0250 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 12 -6 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0056 0.0028 5.8905 --0.0111 0.0056 5.8905 --0.0167 0.0083 5.8905 --0.0222 0.0111 5.8905 --0.0278 0.0139 5.8905 --0.0333 0.0167 5.8905 --0.0389 0.0194 5.8905 --0.0444 0.0222 5.8905 --0.0500 0.0250 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 10 -8 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 14 -4 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/mir_navigation/mprim/unicycle_highcost_2cm.mprim b/mir_navigation/mprim/unicycle_highcost_2cm.mprim deleted file mode 100644 index b87c3105..00000000 --- a/mir_navigation/mprim/unicycle_highcost_2cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.020000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0022 0.0000 0.0000 -0.0044 0.0000 0.0000 -0.0067 0.0000 0.0000 -0.0089 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0133 0.0000 0.0000 -0.0156 0.0000 0.0000 -0.0178 0.0000 0.0000 -0.0200 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 20 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0022 0.0000 0.0000 --0.0044 0.0000 0.0000 --0.0067 0.0000 0.0000 --0.0089 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0133 0.0000 0.0000 --0.0156 0.0000 0.0000 --0.0178 0.0000 0.0000 --0.0200 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 15 3 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0342 0.0008 0.0434 -0.0683 0.0031 0.0868 -0.1023 0.0069 0.1302 -0.1361 0.0121 0.1736 -0.1696 0.0188 0.2170 -0.2029 0.0270 0.2604 -0.2357 0.0366 0.3038 -0.2681 0.0476 0.3472 -0.3000 0.0600 0.3906 -primID: 4 -startangle_c: 0 -endpose_c: 15 -3 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0342 -0.0008 -0.0434 -0.0683 -0.0031 -0.0868 -0.1023 -0.0069 -0.1302 -0.1361 -0.0121 -0.1736 -0.1696 -0.0188 -0.2170 -0.2029 -0.0270 -0.2604 -0.2357 -0.0366 -0.3038 -0.2681 -0.0476 -0.3472 -0.3000 -0.0600 -0.3906 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0044 0.0022 0.3927 -0.0089 0.0044 0.3927 -0.0133 0.0067 0.3927 -0.0178 0.0089 0.3927 -0.0222 0.0111 0.3927 -0.0267 0.0133 0.3927 -0.0311 0.0156 0.3927 -0.0356 0.0178 0.3927 -0.0400 0.0200 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 15 8 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0178 0.3927 -0.0667 0.0356 0.3927 -0.1000 0.0533 0.3927 -0.1333 0.0711 0.3927 -0.1667 0.0889 0.3927 -0.2000 0.1067 0.3927 -0.2333 0.1244 0.3927 -0.2667 0.1422 0.3927 -0.3000 0.1600 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0044 -0.0022 0.3927 --0.0089 -0.0044 0.3927 --0.0133 -0.0067 0.3927 --0.0178 -0.0089 0.3927 --0.0222 -0.0111 0.3927 --0.0267 -0.0133 0.3927 --0.0311 -0.0156 0.3927 --0.0356 -0.0178 0.3927 --0.0400 -0.0200 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 12 10 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0285 0.0188 0.4210 -0.0566 0.0385 0.4492 -0.0842 0.0590 0.4775 -0.1115 0.0804 0.5057 -0.1382 0.1027 0.5340 -0.1645 0.1258 0.5623 -0.1902 0.1497 0.5905 -0.2154 0.1745 0.6188 -0.2400 0.2000 0.6470 -primID: 4 -startangle_c: 1 -endpose_c: 18 5 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0387 0.0160 0.3927 -0.0774 0.0320 0.3927 -0.1161 0.0481 0.3927 -0.1549 0.0636 0.3512 -0.1947 0.0766 0.2809 -0.2353 0.0868 0.2107 -0.2765 0.0941 0.1405 -0.3182 0.0985 0.0702 -0.3600 0.1000 0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0022 0.0022 0.7854 -0.0044 0.0044 0.7854 -0.0067 0.0067 0.7854 -0.0089 0.0089 0.7854 -0.0111 0.0111 0.7854 -0.0133 0.0133 0.7854 -0.0156 0.0156 0.7854 -0.0178 0.0178 0.7854 -0.0200 0.0200 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 15 15 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0022 -0.0022 0.7854 --0.0044 -0.0044 0.7854 --0.0067 -0.0067 0.7854 --0.0089 -0.0089 0.7854 --0.0111 -0.0111 0.7854 --0.0133 -0.0133 0.7854 --0.0156 -0.0156 0.7854 --0.0178 -0.0178 0.7854 --0.0200 -0.0200 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 12 18 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0334 0.0350 0.8288 -0.0652 0.0714 0.8722 -0.0954 0.1092 0.9156 -0.1240 0.1482 0.9590 -0.1508 0.1885 1.0024 -0.1759 0.2299 1.0458 -0.1991 0.2723 1.0892 -0.2205 0.3157 1.1326 -0.2400 0.3600 1.1760 -primID: 4 -startangle_c: 2 -endpose_c: 18 12 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0350 0.0334 0.7420 -0.0714 0.0652 0.6986 -0.1092 0.0954 0.6552 -0.1482 0.1240 0.6118 -0.1885 0.1508 0.5684 -0.2299 0.1759 0.5250 -0.2723 0.1991 0.4816 -0.3157 0.2205 0.4382 -0.3600 0.2400 0.3948 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0022 0.0044 1.1781 -0.0044 0.0089 1.1781 -0.0067 0.0133 1.1781 -0.0089 0.0178 1.1781 -0.0111 0.0222 1.1781 -0.0133 0.0267 1.1781 -0.0156 0.0311 1.1781 -0.0178 0.0356 1.1781 -0.0200 0.0400 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 8 15 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0178 0.0333 1.1781 -0.0356 0.0667 1.1781 -0.0533 0.1000 1.1781 -0.0711 0.1333 1.1781 -0.0889 0.1667 1.1781 -0.1067 0.2000 1.1781 -0.1244 0.2333 1.1781 -0.1422 0.2667 1.1781 -0.1600 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0022 -0.0044 1.1781 --0.0044 -0.0089 1.1781 --0.0067 -0.0133 1.1781 --0.0089 -0.0178 1.1781 --0.0111 -0.0222 1.1781 --0.0133 -0.0267 1.1781 --0.0156 -0.0311 1.1781 --0.0178 -0.0356 1.1781 --0.0200 -0.0400 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 10 12 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0188 0.0285 1.1498 -0.0385 0.0566 1.1216 -0.0590 0.0842 1.0933 -0.0804 0.1115 1.0651 -0.1027 0.1382 1.0368 -0.1258 0.1645 1.0085 -0.1497 0.1902 0.9803 -0.1745 0.2154 0.9520 -0.2000 0.2400 0.9238 -primID: 4 -startangle_c: 3 -endpose_c: 5 18 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0160 0.0387 1.1781 -0.0320 0.0774 1.1781 -0.0481 0.1161 1.1781 -0.0636 0.1549 1.2196 -0.0766 0.1947 1.2898 -0.0868 0.2353 1.3601 -0.0941 0.2765 1.4303 -0.0985 0.3182 1.5006 -0.1000 0.3600 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0022 1.5708 -0.0000 0.0044 1.5708 -0.0000 0.0067 1.5708 -0.0000 0.0089 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0133 1.5708 -0.0000 0.0156 1.5708 -0.0000 0.0178 1.5708 -0.0000 0.0200 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 20 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0022 1.5708 -0.0000 -0.0044 1.5708 -0.0000 -0.0067 1.5708 -0.0000 -0.0089 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0133 1.5708 -0.0000 -0.0156 1.5708 -0.0000 -0.0178 1.5708 -0.0000 -0.0200 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -3 15 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0008 0.0342 1.6142 --0.0031 0.0683 1.6576 --0.0069 0.1023 1.7010 --0.0121 0.1361 1.7444 --0.0188 0.1696 1.7878 --0.0270 0.2029 1.8312 --0.0366 0.2357 1.8746 --0.0476 0.2681 1.9180 --0.0600 0.3000 1.9614 -primID: 4 -startangle_c: 4 -endpose_c: 3 15 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0008 0.0342 1.5274 -0.0031 0.0683 1.4840 -0.0069 0.1023 1.4406 -0.0121 0.1361 1.3972 -0.0188 0.1696 1.3538 -0.0270 0.2029 1.3104 -0.0366 0.2357 1.2670 -0.0476 0.2681 1.2236 -0.0600 0.3000 1.1802 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0022 0.0044 1.9635 --0.0044 0.0089 1.9635 --0.0067 0.0133 1.9635 --0.0089 0.0178 1.9635 --0.0111 0.0222 1.9635 --0.0133 0.0267 1.9635 --0.0156 0.0311 1.9635 --0.0178 0.0356 1.9635 --0.0200 0.0400 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -8 15 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0178 0.0333 1.9635 --0.0356 0.0667 1.9635 --0.0533 0.1000 1.9635 --0.0711 0.1333 1.9635 --0.0889 0.1667 1.9635 --0.1067 0.2000 1.9635 --0.1244 0.2333 1.9635 --0.1422 0.2667 1.9635 --0.1600 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0022 -0.0044 1.9635 -0.0044 -0.0089 1.9635 -0.0067 -0.0133 1.9635 -0.0089 -0.0178 1.9635 -0.0111 -0.0222 1.9635 -0.0133 -0.0267 1.9635 -0.0156 -0.0311 1.9635 -0.0178 -0.0356 1.9635 -0.0200 -0.0400 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -10 12 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0188 0.0285 1.9918 --0.0385 0.0566 2.0200 --0.0590 0.0842 2.0483 --0.0804 0.1115 2.0765 --0.1027 0.1382 2.1048 --0.1258 0.1645 2.1330 --0.1497 0.1902 2.1613 --0.1745 0.2154 2.1896 --0.2000 0.2400 2.2178 -primID: 4 -startangle_c: 5 -endpose_c: -5 18 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0160 0.0387 1.9635 --0.0320 0.0774 1.9635 --0.0481 0.1161 1.9635 --0.0636 0.1549 1.9220 --0.0766 0.1947 1.8517 --0.0868 0.2353 1.7815 --0.0941 0.2765 1.7113 --0.0985 0.3182 1.6410 --0.1000 0.3600 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0022 0.0022 2.3562 --0.0044 0.0044 2.3562 --0.0067 0.0067 2.3562 --0.0089 0.0089 2.3562 --0.0111 0.0111 2.3562 --0.0133 0.0133 2.3562 --0.0156 0.0156 2.3562 --0.0178 0.0178 2.3562 --0.0200 0.0200 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -15 15 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0022 -0.0022 2.3562 -0.0044 -0.0044 2.3562 -0.0067 -0.0067 2.3562 -0.0089 -0.0089 2.3562 -0.0111 -0.0111 2.3562 -0.0133 -0.0133 2.3562 -0.0156 -0.0156 2.3562 -0.0178 -0.0178 2.3562 -0.0200 -0.0200 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -18 12 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0350 0.0334 2.3996 --0.0714 0.0652 2.4430 --0.1092 0.0954 2.4864 --0.1482 0.1240 2.5298 --0.1885 0.1508 2.5732 --0.2299 0.1759 2.6166 --0.2723 0.1991 2.6600 --0.3157 0.2205 2.7034 --0.3600 0.2400 2.7468 -primID: 4 -startangle_c: 6 -endpose_c: -12 18 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0334 0.0350 2.3128 --0.0652 0.0714 2.2694 --0.0954 0.1092 2.2260 --0.1240 0.1482 2.1826 --0.1508 0.1885 2.1392 --0.1759 0.2299 2.0958 --0.1991 0.2723 2.0524 --0.2205 0.3157 2.0090 --0.2400 0.3600 1.9656 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0044 0.0022 2.7489 --0.0089 0.0044 2.7489 --0.0133 0.0067 2.7489 --0.0178 0.0089 2.7489 --0.0222 0.0111 2.7489 --0.0267 0.0133 2.7489 --0.0311 0.0156 2.7489 --0.0356 0.0178 2.7489 --0.0400 0.0200 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -15 8 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0178 2.7489 --0.0667 0.0356 2.7489 --0.1000 0.0533 2.7489 --0.1333 0.0711 2.7489 --0.1667 0.0889 2.7489 --0.2000 0.1067 2.7489 --0.2333 0.1244 2.7489 --0.2667 0.1422 2.7489 --0.3000 0.1600 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0044 -0.0022 2.7489 -0.0089 -0.0044 2.7489 -0.0133 -0.0067 2.7489 -0.0178 -0.0089 2.7489 -0.0222 -0.0111 2.7489 -0.0267 -0.0133 2.7489 -0.0311 -0.0156 2.7489 -0.0356 -0.0178 2.7489 -0.0400 -0.0200 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -12 10 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0285 0.0188 2.7206 --0.0566 0.0385 2.6924 --0.0842 0.0590 2.6641 --0.1115 0.0804 2.6359 --0.1382 0.1027 2.6076 --0.1645 0.1258 2.5793 --0.1902 0.1497 2.5511 --0.2154 0.1745 2.5228 --0.2400 0.2000 2.4946 -primID: 4 -startangle_c: 7 -endpose_c: -18 5 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0387 0.0160 2.7489 --0.0774 0.0320 2.7489 --0.1161 0.0481 2.7489 --0.1549 0.0636 2.7904 --0.1947 0.0766 2.8606 --0.2353 0.0868 2.9309 --0.2765 0.0941 3.0011 --0.3182 0.0985 3.0714 --0.3600 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0022 0.0000 3.1416 --0.0044 0.0000 3.1416 --0.0067 0.0000 3.1416 --0.0089 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0133 0.0000 3.1416 --0.0156 0.0000 3.1416 --0.0178 0.0000 3.1416 --0.0200 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -20 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0022 0.0000 3.1416 -0.0044 0.0000 3.1416 -0.0067 0.0000 3.1416 -0.0089 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0133 0.0000 3.1416 -0.0156 0.0000 3.1416 -0.0178 0.0000 3.1416 -0.0200 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -15 -3 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0342 -0.0008 3.1850 --0.0683 -0.0031 3.2284 --0.1023 -0.0069 3.2718 --0.1361 -0.0121 3.3152 --0.1696 -0.0188 3.3586 --0.2029 -0.0270 3.4020 --0.2357 -0.0366 3.4454 --0.2681 -0.0476 3.4888 --0.3000 -0.0600 3.5322 -primID: 4 -startangle_c: 8 -endpose_c: -15 3 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0342 0.0008 3.0982 --0.0683 0.0031 3.0548 --0.1023 0.0069 3.0114 --0.1361 0.0121 2.9680 --0.1696 0.0188 2.9246 --0.2029 0.0270 2.8812 --0.2357 0.0366 2.8378 --0.2681 0.0476 2.7944 --0.3000 0.0600 2.7510 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0044 -0.0022 3.5343 --0.0089 -0.0044 3.5343 --0.0133 -0.0067 3.5343 --0.0178 -0.0089 3.5343 --0.0222 -0.0111 3.5343 --0.0267 -0.0133 3.5343 --0.0311 -0.0156 3.5343 --0.0356 -0.0178 3.5343 --0.0400 -0.0200 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -15 -8 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0178 3.5343 --0.0667 -0.0356 3.5343 --0.1000 -0.0533 3.5343 --0.1333 -0.0711 3.5343 --0.1667 -0.0889 3.5343 --0.2000 -0.1067 3.5343 --0.2333 -0.1244 3.5343 --0.2667 -0.1422 3.5343 --0.3000 -0.1600 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0044 0.0022 3.5343 -0.0089 0.0044 3.5343 -0.0133 0.0067 3.5343 -0.0178 0.0089 3.5343 -0.0222 0.0111 3.5343 -0.0267 0.0133 3.5343 -0.0311 0.0156 3.5343 -0.0356 0.0178 3.5343 -0.0400 0.0200 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -12 -10 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0285 -0.0188 3.5626 --0.0566 -0.0385 3.5908 --0.0842 -0.0590 3.6191 --0.1115 -0.0804 3.6473 --0.1382 -0.1027 3.6756 --0.1645 -0.1258 3.7038 --0.1902 -0.1497 3.7321 --0.2154 -0.1745 3.7604 --0.2400 -0.2000 3.7886 -primID: 4 -startangle_c: 9 -endpose_c: -18 -5 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0387 -0.0160 3.5343 --0.0774 -0.0320 3.5343 --0.1161 -0.0481 3.5343 --0.1549 -0.0636 3.4928 --0.1947 -0.0766 3.4225 --0.2353 -0.0868 3.3523 --0.2765 -0.0941 3.2821 --0.3182 -0.0985 3.2118 --0.3600 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0022 -0.0022 3.9270 --0.0044 -0.0044 3.9270 --0.0067 -0.0067 3.9270 --0.0089 -0.0089 3.9270 --0.0111 -0.0111 3.9270 --0.0133 -0.0133 3.9270 --0.0156 -0.0156 3.9270 --0.0178 -0.0178 3.9270 --0.0200 -0.0200 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -15 -15 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0022 0.0022 3.9270 -0.0044 0.0044 3.9270 -0.0067 0.0067 3.9270 -0.0089 0.0089 3.9270 -0.0111 0.0111 3.9270 -0.0133 0.0133 3.9270 -0.0156 0.0156 3.9270 -0.0178 0.0178 3.9270 -0.0200 0.0200 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -12 -18 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0334 -0.0350 3.9704 --0.0652 -0.0714 4.0138 --0.0954 -0.1092 4.0572 --0.1240 -0.1482 4.1006 --0.1508 -0.1885 4.1440 --0.1759 -0.2299 4.1874 --0.1991 -0.2723 4.2308 --0.2205 -0.3157 4.2742 --0.2400 -0.3600 4.3176 -primID: 4 -startangle_c: 10 -endpose_c: -18 -12 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0350 -0.0334 3.8836 --0.0714 -0.0652 3.8402 --0.1092 -0.0954 3.7968 --0.1482 -0.1240 3.7534 --0.1885 -0.1508 3.7100 --0.2299 -0.1759 3.6666 --0.2723 -0.1991 3.6232 --0.3157 -0.2205 3.5798 --0.3600 -0.2400 3.5364 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0022 -0.0044 4.3197 --0.0044 -0.0089 4.3197 --0.0067 -0.0133 4.3197 --0.0089 -0.0178 4.3197 --0.0111 -0.0222 4.3197 --0.0133 -0.0267 4.3197 --0.0156 -0.0311 4.3197 --0.0178 -0.0356 4.3197 --0.0200 -0.0400 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -8 -15 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0178 -0.0333 4.3197 --0.0356 -0.0667 4.3197 --0.0533 -0.1000 4.3197 --0.0711 -0.1333 4.3197 --0.0889 -0.1667 4.3197 --0.1067 -0.2000 4.3197 --0.1244 -0.2333 4.3197 --0.1422 -0.2667 4.3197 --0.1600 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0022 0.0044 4.3197 -0.0044 0.0089 4.3197 -0.0067 0.0133 4.3197 -0.0089 0.0178 4.3197 -0.0111 0.0222 4.3197 -0.0133 0.0267 4.3197 -0.0156 0.0311 4.3197 -0.0178 0.0356 4.3197 -0.0200 0.0400 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -10 -12 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0188 -0.0285 4.2914 --0.0385 -0.0566 4.2632 --0.0590 -0.0842 4.2349 --0.0804 -0.1115 4.2067 --0.1027 -0.1382 4.1784 --0.1258 -0.1645 4.1501 --0.1497 -0.1902 4.1219 --0.1745 -0.2154 4.0936 --0.2000 -0.2400 4.0654 -primID: 4 -startangle_c: 11 -endpose_c: -5 -18 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0160 -0.0387 4.3197 --0.0320 -0.0774 4.3197 --0.0481 -0.1161 4.3197 --0.0636 -0.1549 4.3612 --0.0766 -0.1947 4.4314 --0.0868 -0.2353 4.5017 --0.0941 -0.2765 4.5719 --0.0985 -0.3182 4.6422 --0.1000 -0.3600 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0022 4.7124 -0.0000 -0.0044 4.7124 -0.0000 -0.0067 4.7124 -0.0000 -0.0089 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0133 4.7124 -0.0000 -0.0156 4.7124 -0.0000 -0.0178 4.7124 -0.0000 -0.0200 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -20 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0022 4.7124 -0.0000 0.0044 4.7124 -0.0000 0.0067 4.7124 -0.0000 0.0089 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0133 4.7124 -0.0000 0.0156 4.7124 -0.0000 0.0178 4.7124 -0.0000 0.0200 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 3 -15 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0008 -0.0342 4.7558 -0.0031 -0.0683 4.7992 -0.0069 -0.1023 4.8426 -0.0121 -0.1361 4.8860 -0.0188 -0.1696 4.9294 -0.0270 -0.2029 4.9728 -0.0366 -0.2357 5.0162 -0.0476 -0.2681 5.0596 -0.0600 -0.3000 5.1030 -primID: 4 -startangle_c: 12 -endpose_c: -3 -15 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0008 -0.0342 4.6690 --0.0031 -0.0683 4.6256 --0.0069 -0.1023 4.5822 --0.0121 -0.1361 4.5388 --0.0188 -0.1696 4.4954 --0.0270 -0.2029 4.4520 --0.0366 -0.2357 4.4086 --0.0476 -0.2681 4.3652 --0.0600 -0.3000 4.3218 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0022 -0.0044 5.1051 -0.0044 -0.0089 5.1051 -0.0067 -0.0133 5.1051 -0.0089 -0.0178 5.1051 -0.0111 -0.0222 5.1051 -0.0133 -0.0267 5.1051 -0.0156 -0.0311 5.1051 -0.0178 -0.0356 5.1051 -0.0200 -0.0400 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 8 -15 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0178 -0.0333 5.1051 -0.0356 -0.0667 5.1051 -0.0533 -0.1000 5.1051 -0.0711 -0.1333 5.1051 -0.0889 -0.1667 5.1051 -0.1067 -0.2000 5.1051 -0.1244 -0.2333 5.1051 -0.1422 -0.2667 5.1051 -0.1600 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0022 0.0044 5.1051 --0.0044 0.0089 5.1051 --0.0067 0.0133 5.1051 --0.0089 0.0178 5.1051 --0.0111 0.0222 5.1051 --0.0133 0.0267 5.1051 --0.0156 0.0311 5.1051 --0.0178 0.0356 5.1051 --0.0200 0.0400 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 10 -12 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0188 -0.0285 5.1333 -0.0385 -0.0566 5.1616 -0.0590 -0.0842 5.1899 -0.0804 -0.1115 5.2181 -0.1027 -0.1382 5.2464 -0.1258 -0.1645 5.2746 -0.1497 -0.1902 5.3029 -0.1745 -0.2154 5.3312 -0.2000 -0.2400 5.3594 -primID: 4 -startangle_c: 13 -endpose_c: 5 -18 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0160 -0.0387 5.1051 -0.0320 -0.0774 5.1051 -0.0481 -0.1161 5.1051 -0.0636 -0.1549 5.0636 -0.0766 -0.1947 4.9933 -0.0868 -0.2353 4.9231 -0.0941 -0.2765 4.8529 -0.0985 -0.3182 4.7826 -0.1000 -0.3600 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0022 -0.0022 5.4978 -0.0044 -0.0044 5.4978 -0.0067 -0.0067 5.4978 -0.0089 -0.0089 5.4978 -0.0111 -0.0111 5.4978 -0.0133 -0.0133 5.4978 -0.0156 -0.0156 5.4978 -0.0178 -0.0178 5.4978 -0.0200 -0.0200 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 15 -15 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0022 0.0022 5.4978 --0.0044 0.0044 5.4978 --0.0067 0.0067 5.4978 --0.0089 0.0089 5.4978 --0.0111 0.0111 5.4978 --0.0133 0.0133 5.4978 --0.0156 0.0156 5.4978 --0.0178 0.0178 5.4978 --0.0200 0.0200 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 18 -12 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0350 -0.0334 5.5412 -0.0714 -0.0652 5.5846 -0.1092 -0.0954 5.6280 -0.1482 -0.1240 5.6714 -0.1885 -0.1508 5.7148 -0.2299 -0.1759 5.7582 -0.2723 -0.1991 5.8016 -0.3157 -0.2205 5.8450 -0.3600 -0.2400 5.8884 -primID: 4 -startangle_c: 14 -endpose_c: 12 -18 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0334 -0.0350 5.4544 -0.0652 -0.0714 5.4110 -0.0954 -0.1092 5.3676 -0.1240 -0.1482 5.3242 -0.1508 -0.1885 5.2808 -0.1759 -0.2299 5.2374 -0.1991 -0.2723 5.1940 -0.2205 -0.3157 5.1506 -0.2400 -0.3600 5.1072 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0044 -0.0022 5.8905 -0.0089 -0.0044 5.8905 -0.0133 -0.0067 5.8905 -0.0178 -0.0089 5.8905 -0.0222 -0.0111 5.8905 -0.0267 -0.0133 5.8905 -0.0311 -0.0156 5.8905 -0.0356 -0.0178 5.8905 -0.0400 -0.0200 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 15 -8 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0178 5.8905 -0.0667 -0.0356 5.8905 -0.1000 -0.0533 5.8905 -0.1333 -0.0711 5.8905 -0.1667 -0.0889 5.8905 -0.2000 -0.1067 5.8905 -0.2333 -0.1244 5.8905 -0.2667 -0.1422 5.8905 -0.3000 -0.1600 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0044 0.0022 5.8905 --0.0089 0.0044 5.8905 --0.0133 0.0067 5.8905 --0.0178 0.0089 5.8905 --0.0222 0.0111 5.8905 --0.0267 0.0133 5.8905 --0.0311 0.0156 5.8905 --0.0356 0.0178 5.8905 --0.0400 0.0200 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 12 -10 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0285 -0.0188 5.8622 -0.0566 -0.0385 5.8340 -0.0842 -0.0590 5.8057 -0.1115 -0.0804 5.7775 -0.1382 -0.1027 5.7492 -0.1645 -0.1258 5.7209 -0.1902 -0.1497 5.6927 -0.2154 -0.1745 5.6644 -0.2400 -0.2000 5.6362 -primID: 4 -startangle_c: 15 -endpose_c: 18 -5 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0387 -0.0160 5.8905 -0.0774 -0.0320 5.8905 -0.1161 -0.0481 5.8905 -0.1549 -0.0636 5.9320 -0.1947 -0.0766 6.0022 -0.2353 -0.0868 6.0725 -0.2765 -0.0941 6.1427 -0.3182 -0.0985 6.2129 -0.3600 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/mir_navigation/mprim/unicycle_highcost_5cm.mprim b/mir_navigation/mprim/unicycle_highcost_5cm.mprim deleted file mode 100644 index bff26883..00000000 --- a/mir_navigation/mprim/unicycle_highcost_5cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0278 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0389 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0500 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 8 1 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 8 -1 -1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0111 -0.0056 0.3927 --0.0222 -0.0111 0.3927 --0.0333 -0.0167 0.3927 --0.0444 -0.0222 0.3927 --0.0556 -0.0278 0.3927 --0.0667 -0.0333 0.3927 --0.0778 -0.0389 0.3927 --0.0889 -0.0444 0.3927 --0.1000 -0.0500 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 7 2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0056 -0.0056 0.7854 --0.0111 -0.0111 0.7854 --0.0167 -0.0167 0.7854 --0.0222 -0.0222 0.7854 --0.0278 -0.0278 0.7854 --0.0333 -0.0333 0.7854 --0.0389 -0.0389 0.7854 --0.0444 -0.0444 0.7854 --0.0500 -0.0500 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 5 7 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 7 5 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0056 -0.0111 1.1781 --0.0111 -0.0222 1.1781 --0.0167 -0.0333 1.1781 --0.0222 -0.0444 1.1781 --0.0278 -0.0556 1.1781 --0.0333 -0.0667 1.1781 --0.0389 -0.0778 1.1781 --0.0444 -0.0889 1.1781 --0.0500 -0.1000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0278 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0389 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0500 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 8 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 1 8 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0056 -0.0111 1.9635 -0.0111 -0.0222 1.9635 -0.0167 -0.0333 1.9635 -0.0222 -0.0444 1.9635 -0.0278 -0.0556 1.9635 -0.0333 -0.0667 1.9635 -0.0389 -0.0778 1.9635 -0.0444 -0.0889 1.9635 -0.0500 -0.1000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0056 -0.0056 2.3562 -0.0111 -0.0111 2.3562 -0.0167 -0.0167 2.3562 -0.0222 -0.0222 2.3562 -0.0278 -0.0278 2.3562 -0.0333 -0.0333 2.3562 -0.0389 -0.0389 2.3562 -0.0444 -0.0444 2.3562 -0.0500 -0.0500 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -7 5 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -5 7 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0111 -0.0056 2.7489 -0.0222 -0.0111 2.7489 -0.0333 -0.0167 2.7489 -0.0444 -0.0222 2.7489 -0.0556 -0.0278 2.7489 -0.0667 -0.0333 2.7489 -0.0778 -0.0389 2.7489 -0.0889 -0.0444 2.7489 -0.1000 -0.0500 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -7 2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0278 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0389 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0500 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -8 -1 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -8 1 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0111 0.0056 3.5343 -0.0222 0.0111 3.5343 -0.0333 0.0167 3.5343 -0.0444 0.0222 3.5343 -0.0556 0.0278 3.5343 -0.0667 0.0333 3.5343 -0.0778 0.0389 3.5343 -0.0889 0.0444 3.5343 -0.1000 0.0500 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -7 -2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0056 0.0056 3.9270 -0.0111 0.0111 3.9270 -0.0167 0.0167 3.9270 -0.0222 0.0222 3.9270 -0.0278 0.0278 3.9270 -0.0333 0.0333 3.9270 -0.0389 0.0389 3.9270 -0.0444 0.0444 3.9270 -0.0500 0.0500 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -5 -7 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -7 -5 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0056 0.0111 4.3197 -0.0111 0.0222 4.3197 -0.0167 0.0333 4.3197 -0.0222 0.0444 4.3197 -0.0278 0.0556 4.3197 -0.0333 0.0667 4.3197 -0.0389 0.0778 4.3197 -0.0444 0.0889 4.3197 -0.0500 0.1000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0278 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0389 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0500 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -8 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -1 -8 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0056 0.0111 5.1051 --0.0111 0.0222 5.1051 --0.0167 0.0333 5.1051 --0.0222 0.0444 5.1051 --0.0278 0.0556 5.1051 --0.0333 0.0667 5.1051 --0.0389 0.0778 5.1051 --0.0444 0.0889 5.1051 --0.0500 0.1000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0056 0.0056 5.4978 --0.0111 0.0111 5.4978 --0.0167 0.0167 5.4978 --0.0222 0.0222 5.4978 --0.0278 0.0278 5.4978 --0.0333 0.0333 5.4978 --0.0389 0.0389 5.4978 --0.0444 0.0444 5.4978 --0.0500 0.0500 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 7 -5 15 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 5 -7 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0111 0.0056 5.8905 --0.0222 0.0111 5.8905 --0.0333 0.0167 5.8905 --0.0444 0.0222 5.8905 --0.0556 0.0278 5.8905 --0.0667 0.0333 5.8905 --0.0778 0.0389 5.8905 --0.0889 0.0444 5.8905 --0.1000 0.0500 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 7 -2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.9341 -0.0000 0.0000 5.9778 -0.0000 0.0000 6.0214 -0.0000 0.0000 6.0650 -0.0000 0.0000 6.1087 -0.0000 0.0000 6.1523 -0.0000 0.0000 6.1959 -0.0000 0.0000 6.2396 -0.0000 0.0000 0.0000 diff --git a/mir_navigation/nodes/acc_finder.py b/mir_navigation/nodes/acc_finder.py deleted file mode 100755 index f8bd4f8e..00000000 --- a/mir_navigation/nodes/acc_finder.py +++ /dev/null @@ -1,75 +0,0 @@ -#!/usr/bin/env python3 - -import rospy - -from geometry_msgs.msg import Twist -from nav_msgs.msg import Odometry - -LIN_MAX = 1.0 -ANG_MAX = 1.5 # adjust this value to the rough maximum angular velocity - -state = 'stopped' -start = rospy.Time(0) - - -def odom_cb(msg): - global state - - twist = msg.twist.twist - t = (rospy.Time.now() - start).to_sec() - - if state == 'wait_for_stop': - if -0.05 < twist.linear.x < 0.05 and -0.1 < twist.angular.z < 0.1: - state = 'stopped' - rospy.loginfo('state transition --> %s', state) - return - - if state == 'backward' and twist.linear.x < -0.9 * LIN_MAX: - rospy.loginfo('backward from 0 to %f m/s in %f sec', twist.linear.x, t) - elif state == 'forward' and twist.linear.x > 0.9 * LIN_MAX: - rospy.loginfo('forward from 0 to %f m/s in %f sec', twist.linear.x, t) - elif state == 'turning_clockwise' and twist.angular.z < -0.9 * ANG_MAX: - rospy.loginfo('turning_clockwise from 0 to %f rad/s in %f sec', twist.angular.z, t) - elif state == 'turning_counter_clockwise' and twist.angular.z > 0.9 * ANG_MAX: - rospy.loginfo('turning_counter_clockwise from 0 to %f rad/s in %f sec', twist.angular.z, t) - else: - return - - state = 'wait_for_stop' - rospy.loginfo('state transition --> %s', state) - - -def cmd_vel_cb(msg): - global state, start - - if state != 'stopped': - return - - if msg.linear.x <= -LIN_MAX: - start = rospy.Time.now() - state = 'backward' - elif msg.linear.x >= LIN_MAX: - start = rospy.Time.now() - state = 'forward' - elif msg.angular.z <= -ANG_MAX: - start = rospy.Time.now() - state = 'turning_clockwise' - elif msg.angular.z >= ANG_MAX: - start = rospy.Time.now() - state = 'turning_counter_clockwise' - else: - return - - rospy.loginfo('state transition --> %s', state) - - -def main(): - rospy.init_node('acc_finder', anonymous=True) - rospy.Subscriber('odom', Odometry, odom_cb) - rospy.Subscriber('cmd_vel', Twist, cmd_vel_cb) - rospy.loginfo('acc_finder node ready and listening. now use teleop to move your robot to the limits!') - rospy.spin() - - -if __name__ == '__main__': - main() diff --git a/mir_navigation/nodes/min_max_finder.py b/mir_navigation/nodes/min_max_finder.py deleted file mode 100755 index 05c562a4..00000000 --- a/mir_navigation/nodes/min_max_finder.py +++ /dev/null @@ -1,34 +0,0 @@ -#!/usr/bin/env python3 -import rospy - -from nav_msgs.msg import Odometry - -lin_min = 0.0 -lin_max = 0.0 -ang_min = 0.0 -ang_max = 0.0 - - -def odom_cb(msg): - global lin_min, lin_max, ang_min, ang_max - if lin_min > msg.twist.twist.linear.x: - lin_min = msg.twist.twist.linear.x - if lin_max < msg.twist.twist.linear.x: - lin_max = msg.twist.twist.linear.x - if ang_min > msg.twist.twist.angular.z: - ang_min = msg.twist.twist.angular.z - if ang_max < msg.twist.twist.angular.z: - ang_max = msg.twist.twist.angular.z - - rospy.loginfo('linear: [%f, %f] angular: [%f, %f]', lin_min, lin_max, ang_min, ang_max) - - -def main(): - rospy.init_node('min_max_finder', anonymous=True) - rospy.Subscriber('odom', Odometry, odom_cb) - rospy.loginfo('min_max_finde node ready and listening. now use teleop to move your robot to the limits!') - rospy.spin() - - -if __name__ == '__main__': - main() diff --git a/mir_navigation/package.xml b/mir_navigation/package.xml index 0022afb6..24587bb5 100644 --- a/mir_navigation/package.xml +++ b/mir_navigation/package.xml @@ -1,5 +1,5 @@ - + mir_navigation 1.1.3 Launch and configuration files for move_base, localization etc. on the MiR robot. @@ -13,25 +13,17 @@ https://github.com/dfki-ric/mir_robot https://github.com/dfki-ric/mir_robot/issues - catkin - - roslaunch - - amcl - base_local_planner - dwa_local_planner - dwb_critics - dwb_local_planner - dwb_plugins - - hector_mapping - map_server - mir_driver - mir_dwb_critics - mir_gazebo - move_base - nav_core_adapter - python3-matplotlib - sbpl_lattice_planner - teb_local_planner + mir_gazebo + rviz2 + nav2_bringup + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mir_navigation/rviz/mir_mapping.rviz b/mir_navigation/rviz/mir_mapping.rviz new file mode 100644 index 00000000..5e79aba8 --- /dev/null +++ b/mir_navigation/rviz/mir_mapping.rviz @@ -0,0 +1,495 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1/Description Topic1 + - /LaserScan3/Topic1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 512 + - 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: slam_toolbox::SlamToolboxPlugin + Name: SlamToolboxPlugin +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: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Angle Tolerance: 0.10000000149011612 + 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: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: false + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + surface: + Alpha: 1 + Show Axes: false + Show Trail: false + us_1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + us_2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + virtual_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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/LaserScan + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /f_scan + 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: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /b_scan + 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: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + back_laser_link: + Value: true + base_footprint: + Value: true + base_link: + Value: true + bl_caster_rotation_link: + Value: true + bl_caster_wheel_link: + Value: true + br_caster_rotation_link: + Value: true + br_caster_wheel_link: + Value: true + fl_caster_rotation_link: + Value: true + fl_caster_wheel_link: + Value: true + fr_caster_rotation_link: + Value: true + fr_caster_wheel_link: + Value: true + front_laser_link: + Value: true + imu_frame: + Value: true + imu_link: + Value: true + left_wheel_link: + Value: true + map: + Value: true + odom: + Value: true + right_wheel_link: + Value: true + surface: + Value: true + us_1_frame: + Value: true + us_2_frame: + Value: true + virtual_laser_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + back_laser_link: + {} + bl_caster_rotation_link: + bl_caster_wheel_link: + {} + br_caster_rotation_link: + br_caster_wheel_link: + {} + fl_caster_rotation_link: + fl_caster_wheel_link: + {} + fr_caster_rotation_link: + fr_caster_wheel_link: + {} + front_laser_link: + {} + imu_link: + imu_frame: + {} + left_wheel_link: + {} + right_wheel_link: + {} + surface: + {} + us_1_frame: + {} + us_2_frame: + {} + virtual_laser_link: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + 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 + 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: 29.811614990234375 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.2774231731891632 + Y: 3.960057497024536 + Z: 5.7105231285095215 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.544796347618103 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.029999636113643646 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1088 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000217000003e6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000023d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000280000001a30000018500ffffff000000010000010f000003e6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003e6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000486000003e600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + SlamToolboxPlugin: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1976 + X: 72 + Y: 27 diff --git a/mir_navigation/rviz/mir_mapping_nav.rviz b/mir_navigation/rviz/mir_mapping_nav.rviz new file mode 100644 index 00000000..4ed390c9 --- /dev/null +++ b/mir_navigation/rviz/mir_mapping_nav.rviz @@ -0,0 +1,504 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1/Description Topic1 + - /LaserScan3/Topic1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 400 + - 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: slam_toolbox::SlamToolboxPlugin + Name: SlamToolboxPlugin +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: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Angle Tolerance: 0.10000000149011612 + 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: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: false + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + surface: + Alpha: 1 + Show Axes: false + Show Trail: false + us_1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + us_2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + virtual_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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/LaserScan + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /f_scan + 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: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /b_scan + 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: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + back_laser_link: + Value: true + base_footprint: + Value: true + base_link: + Value: true + bl_caster_rotation_link: + Value: true + bl_caster_wheel_link: + Value: true + br_caster_rotation_link: + Value: true + br_caster_wheel_link: + Value: true + fl_caster_rotation_link: + Value: true + fl_caster_wheel_link: + Value: true + fr_caster_rotation_link: + Value: true + fr_caster_wheel_link: + Value: true + front_laser_link: + Value: true + imu_frame: + Value: true + imu_link: + Value: true + left_wheel_link: + Value: true + right_wheel_link: + Value: true + surface: + Value: true + us_1_frame: + Value: true + us_2_frame: + Value: true + virtual_laser_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + 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 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + 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 + 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: 29.811614990234375 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.1666731983423233 + Y: 3.7174344062805176 + Z: 5.710631847381592 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.2781853675842285 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000002170000039efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000213000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e0100000256000001850000018500ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004060000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + SlamToolboxPlugin: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/mir_navigation/rviz/mir_nav.rviz b/mir_navigation/rviz/mir_nav.rviz new file mode 100644 index 00000000..510d5b3c --- /dev/null +++ b/mir_navigation/rviz/mir_nav.rviz @@ -0,0 +1,501 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1/Description Topic1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 859 + - 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 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + 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: 10 + Reference Frame: + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Angle Tolerance: 0.10000000149011612 + 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: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: false + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + surface: + Alpha: 1 + Show Axes: false + Show Trail: false + us_1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + us_2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + virtual_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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/LaserScan + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /f_scan + 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: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /b_scan + 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: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + back_laser_link: + Value: true + base_footprint: + Value: true + base_link: + Value: true + bl_caster_rotation_link: + Value: true + bl_caster_wheel_link: + Value: true + br_caster_rotation_link: + Value: true + br_caster_wheel_link: + Value: true + fl_caster_rotation_link: + Value: true + fl_caster_wheel_link: + Value: true + fr_caster_rotation_link: + Value: true + fr_caster_wheel_link: + Value: true + front_laser_link: + Value: true + imu_frame: + Value: true + imu_link: + Value: true + left_wheel_link: + Value: true + odom: + Value: true + right_wheel_link: + Value: true + surface: + Value: true + us_1_frame: + Value: true + us_2_frame: + Value: true + virtual_laser_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + 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 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + 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 + 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: 36.69948196411133 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.11027210205793381 + Y: 2.4609313011169434 + Z: 5.693253993988037 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5497963428497314 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.278184413909912 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1088 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001ea000003e6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003e6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003e6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003e6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004b3000003e600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1976 + X: 72 + Y: 27 diff --git a/mir_navigation/rviz/mir_visu.rviz b/mir_navigation/rviz/mir_visu.rviz new file mode 100644 index 00000000..6ee22096 --- /dev/null +++ b/mir_navigation/rviz/mir_visu.rviz @@ -0,0 +1,341 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1/Description Topic1 + Splitter Ratio: 0.5 + Tree Height: 784 + - 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 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + 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: 10 + Reference Frame: + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Angle Tolerance: 0.10000000149011612 + 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: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + 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: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: false + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + surface: + Alpha: 1 + Show Axes: false + Show Trail: false + us_1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + us_2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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/LaserScan + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.08 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + 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 + 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: 51.56013107299805 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.1365268975496292 + Y: 3.939230442047119 + Z: 5.688584327697754 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.283184051513672 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1013 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001ea0000039bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000038b0000039b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1680 + X: 1920 + Y: 0 diff --git a/mir_navigation/rviz/navigation.rviz b/mir_navigation/rviz/navigation.rviz deleted file mode 100644 index 528ff108..00000000 --- a/mir_navigation/rviz/navigation.rviz +++ /dev/null @@ -1,497 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 81 - Name: Displays - Property Tree Widget: - Expanded: - - /Grid1 - - /Grid1/Offset1 - - /TF1/Frames1 - - /TF1/Tree1 - - /Local Map1/DWB Markers1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 575 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: LaserScan -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 25 - Y: 25 - Z: 0 - Plane: XY - Plane Cell Count: 50 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - back_laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bl_caster_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - br_caster_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fl_caster_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_rotation_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fr_caster_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - front_laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - left_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - surface: - Alpha: 1 - Show Axes: false - Show Trail: false - us_1_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - us_2_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - 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/LaserScan - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.0500000007 - Style: Squares - Topic: scan - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - 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/PointCloud2 - Color: 255; 0; 0 - 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: Bumper Hit - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.0799999982 - Style: Spheres - Topic: mobile_base/sensors/bumper_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 0.699999988 - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: /map - Unreliable: false - Use Timestamp: false - Value: true - - Class: rviz/Group - Displays: - - Alpha: 0.699999988 - Class: rviz/Map - Color Scheme: costmap - Draw Behind: true - Enabled: true - Name: Costmap - Topic: move_base_node/global_costmap/costmap - Unreliable: false - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Billboards - Line Width: 0.0500000007 - Name: Global Plan (DWB) - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: move_base_node/DWBLocalPlanner/transformed_global_plan - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Billboards - Line Width: 0.0500000007 - Name: Full Plan - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: move_base_node/SBPLLatticePlanner/plan - Unreliable: false - Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 255; 0; 0 - Enabled: true - Name: Footprint - Topic: move_base_node/global_costmap/footprint - Unreliable: false - Value: true - Enabled: true - Name: Global Map - - Class: rviz/Group - Displays: - - Alpha: 0.699999988 - Class: rviz/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Costmap - Topic: move_base_node/local_costmap/costmap - Unreliable: false - Use Timestamp: false - Value: true - - Alpha: 1 - Class: rviz/GridCells - Color: 25; 255; 0 - Enabled: true - Name: Costmap (MiR) - Topic: move_base_node/local_costmap/obstacles - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 12; 255 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Billboards - Line Width: 0.0500000007 - Name: Local Plan (DWB) - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: move_base_node/DWBLocalPlanner/local_plan - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 12; 255 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Billboards - Line Width: 0.0500000007 - Name: Local Plan (MiR) - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: move_base_node/MIRPlannerROS/local_plan - Unreliable: false - Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 25; 255; 0 - Enabled: true - Name: Footprint - Topic: move_base_node/local_costmap/footprint - Unreliable: false - Value: true - - Alpha: 0.300000012 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: total_cost - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 40 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Cost Cloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.0399999991 - Style: Flat Squares - Topic: move_base_node/DWBLocalPlanner/cost_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: move_base_node/DWBLocalPlanner/markers - Name: DWB Markers - Namespaces: - InvalidTrajectories: true - ValidTrajectories: true - Queue Size: 100 - Value: true - Enabled: true - Name: Local Map - - Alpha: 1 - Arrow Length: 0.200000003 - Axes Length: 0.300000012 - Axes Radius: 0.00999999978 - Class: rviz/PoseArray - Color: 0; 192; 0 - Enabled: true - Head Length: 0.0700000003 - Head Radius: 0.0299999993 - Name: Amcl Particle Swarm - Shaft Length: 0.230000004 - Shaft Radius: 0.00999999978 - Shape: Arrow (Flat) - Topic: particlecloud - Unreliable: false - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/MoveCamera - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/Select - - Class: rviz/SetInitialPose - Topic: initialpose - - Class: rviz/SetGoal - Topic: move_base_simple/goal - - Class: rviz/Measure - - Class: rviz/PublishPoint - Single click: true - Topic: clicked_point - Value: true - Views: - Current: - Angle: 0 - Class: rviz/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Scale: 68.8703232 - Target Frame: - Value: TopDownOrtho (rviz) - X: 13.2673092 - Y: 16.1651554 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 818 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000002d1fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002d1000000e300ffffff000000010000010f000002d1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002d1000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000022300fffffffb0000000800540069006d006501000000000000045000000000000000000000034b000002d100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1488 - X: 254 - Y: 36 diff --git a/mir_navigation/scripts/plot_mprim.py b/mir_navigation/scripts/plot_mprim.py deleted file mode 100755 index 37e0369c..00000000 --- a/mir_navigation/scripts/plot_mprim.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python3 - -import math -import matplotlib.pyplot as plt -import numpy as np -import sys - - -def get_value(strline, name): - if strline.find(name) < 0: - raise Exception("File format not matching the parser expectation", name) - - return strline.replace(name, "", 1) - - -def get_pose(line): - ss = line.split() - return np.array([float(ss[0]), float(ss[1]), float(ss[2])]) - - -class MPrim: - def __init__(self, f): - self.primID = int(get_value(f.readline(), "primID:")) - self.startAngle = int(get_value(f.readline(), "startangle_c:")) - self.endPose = get_pose(get_value(f.readline(), "endpose_c:")) - self.cost = float(get_value(f.readline(), "additionalactioncostmult:")) - self.nrPoses = int(get_value(f.readline(), "intermediateposes:")) - poses = [] - for _ in range(self.nrPoses): - poses.append(f.readline()) - self.poses = np.loadtxt(poses, delimiter=" ") - self.cmap = plt.get_cmap("nipy_spectral") - - def plot(self, nr_angles): - plt.plot(self.poses[:, 0], self.poses[:, 1], c=self.cmap(float(self.startAngle) / nr_angles)) - - -class MPrims: - def __init__(self, filename): - f = open(filename, "r") - - self.resolution = float(get_value(f.readline(), "resolution_m:")) - self.nrAngles = int(get_value(f.readline(), "numberofangles:")) - self.nrPrims = int(get_value(f.readline(), "totalnumberofprimitives:")) - - self.prims = [] - for _ in range(self.nrPrims): - self.prims.append(MPrim(f)) - - f.close() - - def plot(self): - fig = plt.figure() - ax = fig.add_subplot(111) - ax.set_xticks(np.arange(-1, 1, self.resolution)) - ax.set_yticks(np.arange(-1, 1, self.resolution)) - for prim in self.prims: - prim.plot(self.nrAngles) - plt.grid() - plt.show() - - -prims = MPrims(sys.argv[1]) -prims.plot() diff --git a/mir_restapi/LICENSE b/mir_restapi/LICENSE new file mode 100644 index 00000000..41194b93 --- /dev/null +++ b/mir_restapi/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2021, niemsoen +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/mir_restapi/README.md b/mir_restapi/README.md new file mode 100644 index 00000000..1ff4c7fd --- /dev/null +++ b/mir_restapi/README.md @@ -0,0 +1,3 @@ +# mir_restapi +ROS server node and client node, that implements calls to the Mir REST API. Currently implemented: +- set MiR's time diff --git a/mir_restapi/mir_restapi/__init__.py b/mir_restapi/mir_restapi/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mir_restapi/mir_restapi/mir_restapi_lib.py b/mir_restapi/mir_restapi/mir_restapi_lib.py new file mode 100755 index 00000000..eab4f6d5 --- /dev/null +++ b/mir_restapi/mir_restapi/mir_restapi_lib.py @@ -0,0 +1,271 @@ +import json +import time +import http.client +from datetime import datetime + + +class HttpConnection(): + + def __init__(self, logger, address, auth, api_prefix): + self.logger = logger + self.api_prefix = api_prefix + self.http_headers = { + "Accept-Language": "en-EN", + "Authorization": auth, + "Content-Type": "application/json"} + try: + self.connection = http.client.HTTPConnection(host=address, timeout=5) + except Exception as e: + self.logger.warn('Creation of http connection failed') + self.logger.warn(str(e)) + + def __del__(self): + if self.is_valid(): + self.connection.close() + + def is_valid(self): + return self.connection is not None + + def get(self, path): + if not self.is_valid(): + self.connection.connect() + self.connection.request("GET", self.api_prefix+path, headers=self.http_headers) + resp = self.connection.getresponse() + if resp.status < 200 or resp.status >= 300: + self.logger.warn("GET failed with status {} and reason: {}".format(resp.status, + resp.reason)) + return resp + + def post(self, path, body): + self.connection.request("POST", self.api_prefix+path, body=body, headers=self.http_headers) + resp = self.connection.getresponse() + if resp.status < 200 or resp.status >= 300: + self.logger.warn("POST failed with status {} and reason: {}".format( + resp.status, resp.reason)) + return json.loads(resp.read()) + + def put(self, path, body): + self.connection.request("PUT", self.api_prefix+path, body=body, headers=self.http_headers) + resp = self.connection.getresponse() + # self.logger.info(resp.read()) + if resp.status < 200 or resp.status >= 300: + self.logger.warn("POST failed with status {} and reason: {}".format( + resp.status, resp.reason)) + return json.loads(resp.read()) + + def put_no_response(self, path, body): + self.connection.request("PUT", self.api_prefix+path, body=body, headers=self.http_headers) + + +class MirRestAPI(): + + def __init__(self, logger, hostname, auth=""): + self.logger = logger + if hostname is not None: + address = hostname + ":80" + # else: + # address="192.168.12.20:80" + self.http = HttpConnection(logger, address, auth, "/api/v2.0.0") + + def close(self): + self.http.__del__() + self.logger.info("REST API: Connection closed") + + def is_connected(self, print=True): + if not self.http.is_valid(): + self.logger.warn('REST API: Http-Connection is not valid') + return False + try: + self.http.connection.connect() + self.http.connection.close() + if print: + self.logger.info("REST API: Connected!") + except Exception as e: + if print: + self.logger.warn('REST API: Attempt to connect failed: ' + str(e)) + return False + return True + + def is_available(self): + status = json.dumps(self.get_status()) + if "service_unavailable" in status: + return False + else: + return True + + def wait_for_available(self): + while True: + if self.is_connected(print=False): + if self.is_available(): + self.logger.info('REST API: available') + break + else: + self.logger.info('REST API: unavailable... waiting') + time.sleep(1) + + def get_status(self): + response = self.http.get("/status") + return json.loads(response.read()) + + def get_state_id(self): + status = self.get_status() + state_id = status["state_id"] + return state_id + """ Choices are: {3, 4, 11}, State: {Ready, Pause, Manualcontrol} + """ + def set_state_id(self, stateId): + return self.http.put("/status", json.dumps({'state_id': stateId})) + + def is_ready(self): + status = self.get_status() + if status["state_id"] != 3: # 3=Ready, 4=Pause, 11=Manualcontrol + self.logger.warn("MIR currently occupied. System state: {}".format( + status["state_text"])) + return False + else: + return True + + def get_all_settings(self, advanced=False, listGroups=False): + if advanced: + response = self.http.get("/settings/advanced") + elif listGroups: + response = self.http.get("/setting_groups") + else: + response = self.http.get("/settings") + return json.loads(response.read()) + + def get_group_settings(self, groupID): + response = self.http.get("/setting_groups/" + groupID + "/settings") + return json.loads(response.read()) + + def set_setting(self, settingID, settingData): + return self.http.put("/setting", json.dumps({settingID: settingData})) + + def sync_time(self): + timeobj = datetime.now() + dT = timeobj.strftime("%Y-%m-%dT%X") + response = 'REST API: ' + try: + response += str(self.http.put("/status", json.dumps({'datetime': dT}))) + except Exception as e: + if str(e) == "timed out": + # setting datetime over REST API seems not to be intended + # that's why there is no response accompanying the PUT request, + # therefore a time out occurs, however time has been set correctly + response += "Set datetime to " + dT + self.logger.warn("REST API: Setting time Mir triggers emergency stop, \ + please unlock.") + self.logger.info(response) + + # this is needed, because a timeset restarts the restAPI + self.wait_for_available() + + return response + response += " Error setting datetime" + return response + + def get_distance_statistics(self): + response = self.http.get("/statistics/distance") + return json.loads(response.read()) + + def get_positions(self): + response = self.http.get("/positions") + return json.loads(response.read()) + + def get_pose_guid(self, pos_name): + positions = self.get_positions() + return next((pos["guid"] for pos in positions if pos["name"] == pos_name), None) + + def get_missions(self): + response = self.http.get("/missions") + return json.loads(response.read()) + + def get_mission_guid(self, mission_name): + missions = self.get_missions() + return next((mis["guid"] for mis in missions if mis["name"] == mission_name), None) + + def get_sounds(self): + response = self.http.get("/sounds") + return json.loads(response.read()) + + def move_to(self, position, mission="move_to"): + mis_guid = self.get_mission_guid(mission) + pos_guid = self.get_pose_guid(position) + + for (var, txt, name) in zip((mis_guid, pos_guid), ("Mission", "Position"), + (mission, position)): + if var is None: + self.logger.warn( + "No {} named {} available on MIR - Aborting move_to".format(txt, name)) + return + + body = json.dumps({ + "mission_id": mis_guid, + "message": "Externally scheduled mission from the MIR Python Client", + "parameters": [{ + "value": pos_guid, + "input_name": "target" + }]}) + + data = self.http.post("/mission_queue", body) + self.logger.info("Mission scheduled for execution under id {}".format(data["id"])) + + while data["state"] != "Done": + resp = self.http.get("/mission_queue/{}".format(data["id"])) + data = json.loads(resp.read()) + if data["state"] == "Error": + self.logger.warn("Mission failed as robot is in error") + return + self.logger.info(data["state"]) + time.sleep(2) + + self.logger.info("Mission executed successfully") + + def add_mission_to_queue(self, mission_name): + mis_guid = self.get_mission_guid(mission_name) + if mis_guid is None: + self.logger.warn( + "No Mission named '{}' available on MIR - Aborting move_to".format(mission_name)) + return False, -1 + + # put in mission queue + body = json.dumps({"mission_id": str(mis_guid), + "message": "Mission scheduled by ROS node mir_restapi_server", + "priority": 0}) + + data = self.http.post("/mission_queue", body) + try: + self.logger.info("Mission scheduled for execution under id {}".format(data["id"])) + return True, int(data["id"]) + except KeyError: + self.logger.warn("Couldn't schedule mission") + self.logger.warn(str(data)) + return False, -1 + + def is_mission_done(self, mission_queue_id): + try: + # mis_guid = self.get_mission_guid(mission_name) + response = self.http.get("/mission_queue") + + except http.client.ResponseNotReady or http.client.CannotSendRequest: + self.logger.info( + "Http error: Mission with queue_id {} is still in queue".format(mission_queue_id)) + self.http.__del__() + return False + + # self.logger.info("Mission with queue_id {} is in queue".format(mission_queue_id)) + # self.logger.info("Response status {}".format(response.status)) + data = json.loads(response.read()) + + for d in data: + if d["id"] == mission_queue_id: + if d["state"] == 'Done': + self.logger.info("Mission {} is done".format(mission_queue_id)) + return True + + self.logger.info("Mission with queue_id {} is still in queue".format(mission_queue_id)) + return False + + def get_system_info(self): + response = self.http.get("/system/info") + return json.loads(response.read()) diff --git a/mir_restapi/mir_restapi/mir_restapi_server.py b/mir_restapi/mir_restapi/mir_restapi_server.py new file mode 100755 index 00000000..a6e437e1 --- /dev/null +++ b/mir_restapi/mir_restapi/mir_restapi_server.py @@ -0,0 +1,255 @@ +import time +import sys + +import rclpy +from rclpy.node import Node + +import mir_restapi.mir_restapi_lib +from std_srvs.srv import Trigger +from mir_msgs.srv import ExecMission +from rcl_interfaces.msg import SetParametersResult + + +class MirRestAPIServer(Node): + + def __init__(self): + super().__init__('mir_restapi_server') + self.get_logger().info("started") + + # parameters: hostname, api_token + self.declare_parameter('mir_hostname', "") + self.hostname = self.get_parameter('mir_hostname').get_parameter_value().string_value + self.declare_parameter('mir_restapi_auth', "") + self.auth = self.get_parameter('mir_restapi_auth').get_parameter_value().string_value + self.add_on_set_parameters_callback(self.parameters_callback) + + self.api_handle = None + + self.setup_api_handle() + + if self.api_handle is None: + self.get_logger().warn(""" + Hostname and API token are not set! Run as follows: + + ros2 run mir_restapi mir_restapi_server + --ros-args -p mir_hostname:='MIR_IP_ADDR' -p mir_restapi_auth:='YOUR_API_KEY' + """) + + def setup_api_handle(self): + if self.hostname != "" and self.auth != "": + self.api_handle = mir_restapi.mir_restapi_lib.MirRestAPI( + self.get_logger(), self.hostname, self.auth) + self.get_logger().info("created MirRestAPI handle") + self.create_services() + self.get_logger().info("created services") + + def parameters_callback(self, params): + for param in params: + if param.name == "mir_restapi_auth": + self.get_logger().info("Received auth token") + self.auth = param.value + if param.name == "mir_hostname": + self.get_logger().info("Set mir hostname") + self.hostname = param.value + self.setup_api_handle() + return SetParametersResult(successful=True) + + def create_services(self): + self.create_service( + Trigger, + 'mir_100_sync_time', + self.sync_time_callback) + self.get_logger().info("Listening on 'mir_100_sync_time'") + + self.create_service( + Trigger, + 'mir_100_get_status', + self.get_status_callback) + self.get_logger().info("Listening on 'mir_100_get_status'") + + self.create_service( + Trigger, + 'mir_100_get_sounds', + self.get_sounds_callback) + self.get_logger().info("Listening on 'mir_100_get_sounds'") + + self.create_service( + Trigger, + 'mir_100_is_emergency_halt', + self.is_emergency_halt_callback) + self.get_logger().info("Listening on 'mir_100_is_emergency_halt'") + + self.create_service( + Trigger, + 'mir_100_get_missions', + self.get_missions_callback) + self.get_logger().info("Listening on 'mir_100_get_missions'") + + self.create_service( + Trigger, + 'mir_100_honk', + self.honk_callback) + self.get_logger().info("Listening on 'mir_100_honk'") + + self.create_service( + ExecMission, + 'mir_100_execute_mission', + self.exec_mission_callback) + self.get_logger().info("Listening on 'mir_100_execute_mission'") + + self.create_service( + Trigger, + 'mir_100_get_system_info', + self.get_system_info_callback) + self.get_logger().info("Listening on 'mir_100_get_system_info'") + + self.create_service( + Trigger, + 'mir_100_get_settings', + self.get_settings_callback) + self.get_logger().info("Listening on 'mir_100_get_settings'") + + def test_api_connection(self): + if self.api_handle is None: + return -1 + + self.get_logger().info('REST API: Waiting for connection') + i = 1 + while not self.api_handle.is_connected(): + if not rclpy.ok(): + sys.exit(0) + if i > 5: + self.get_logger().error('REST API: Could not connect, giving up') + return 0 + i += 1 + time.sleep(1) + return 1 + + def reponse_api_handle_not_exists(self, response): + response.success = False + response.message = 'API token and/or hostname not set yet' + self.get_logger().error(response.message) + return response + + def call_restapi_function(self, service_fct, request, response, args=None): + if self.test_api_connection() == -1: + response = self.reponse_api_handle_not_exists(response) + return response + if self.api_handle.is_connected(print=False): + if args is None: + response.message = str(service_fct()) + else: + response.message = str(service_fct(args)) + if "Error" in response.message: + response.success = False + else: + response.success = True + return response + else: + response.success = False + response.message = "ERROR: Couldn't connect to REST API" + self.get_logger().error(response.message) + return response + + def sync_time_callback(self, request, response): + self.get_logger().info('Syncing host time with REST API...') + response = self.call_restapi_function(self.api_handle.sync_time, request, response) + return response + + def get_status_callback(self, request, response): + self.get_logger().info('Getting status from REST API...') + response = self.call_restapi_function(self.api_handle.get_status, request, response) + return response + + def get_sounds_callback(self, request, response): + self.get_logger().info('Getting sounds from REST API...') + response = self.call_restapi_function(self.api_handle.get_sounds, request, response) + return response + + def is_emergency_halt_callback(self, request, response): + self.get_logger().info('Checking REST API for emergency halt...') + response = self.call_restapi_function(self.api_handle.get_state_id, request, response) + + if response.success: + state_id = int(response.message) + # self.get_logger().info("Returned state_id as %i" % state_id) + STATE_ID_EMERGENCY = 10 + if state_id == STATE_ID_EMERGENCY: + response.message = str(True) + self.get_logger().info("Emergency Halt") + else: + response.message = str(False) + # self.get_logger().info("no emergency halt") + return response + + def get_missions_callback(self, request, response): + self.get_logger().info('Getting missions from REST API...') + response = self.call_restapi_function(self.api_handle.get_missions, request, response) + return response + + def honk_callback(self,request,response): + + req = ExecMission.Request() + req.mission_name = "honk" + resp = ExecMission.Response() + + self.exec_mission_callback(req, resp) + response.success = resp.success + return response + + def exec_mission_callback(self, request, response): + + mission_name = request.mission_name + + queue_success, mission_queue_id = self.api_handle.add_mission_to_queue(mission_name) + if not queue_success: + response.message = "Execution Mission '{}' failed due to mission queue error".format(mission_name) + self.get_logger().error(response.message) + response.success = False + return response + self.get_logger().info("Put mission {} into queue with mission_queue_id={}".format(mission_name, + mission_queue_id)) + + emerg_response = self.is_emergency_halt_callback(request, response) + if emerg_response.message == str(True): + response.message = "Can't execute mission, emergency halt" + self.get_logger().error(response.message) + response.success = False + else: + response.message = "Executing mission '{}'".format(mission_name) + self.get_logger().info(response.message) + STATE_ID_RUN_MISSION = 3 + STATE_ID_PAUSE = 4 + + self.api_handle.set_state_id(STATE_ID_RUN_MISSION) + + while not self.api_handle.is_mission_done(mission_queue_id): + time.sleep(1) + + self.api_handle.set_state_id(STATE_ID_PAUSE) + self.api_handle.http.__del__() + response.success = True + return response + + def get_system_info_callback(self, request, response): + self.get_logger().info('Getting system info from REST API...') + response = self.call_restapi_function(self.api_handle.get_system_info, request, response) + return response + + def get_settings_callback(self, request, response): + self.get_logger().info('Getting settings from REST API...') + response = self.call_restapi_function(self.api_handle.get_all_settings, request, response) + return response + + +def main(args=None): + rclpy.init(args=args) + + mir_restapi_server = MirRestAPIServer() + + rclpy.spin(mir_restapi_server) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/mir_restapi/mir_restapi/mir_restapi_sync_time.py b/mir_restapi/mir_restapi/mir_restapi_sync_time.py new file mode 100644 index 00000000..65f72180 --- /dev/null +++ b/mir_restapi/mir_restapi/mir_restapi_sync_time.py @@ -0,0 +1,55 @@ +import rclpy +from rclpy.node import Node +from std_srvs.srv import Trigger + + +class MirRestAPIClient(Node): + + def __init__(self): + super().__init__('mir_restapi_client_sync_time') + self.create_api_clients() + + self.restAPI_setTime.wait_for_service() + + self.sync_time() + rclpy.shutdown() + exit() + + def create_api_clients(self): + self.restAPI_setTime = self.create_client( + Trigger, + 'mir_100_sync_time') + + self.restAPI_getStatus = self.create_client( + Trigger, + 'mir_100_get_status') + + def call_trigger_service(self, client): + req = Trigger.Request() + future = client.call_async(req) + rclpy.spin_until_future_complete(self, future, timeout_sec=60) + if not future.done(): + self.get_logger().error("timeout") + else: + # service done + self.get_logger().info("service executed") + res = future.result() + if res.success: + self.get_logger().info(res.message) + else: + self.get_logger().error(res.message) + + def sync_time(self): + self.call_trigger_service(self.restAPI_setTime) + + def get_status(self): + self.call_trigger_service(self.restAPI_getStatus) + + +def main(args=None): + rclpy.init(args=args) + MirRestAPIClient() + + +if __name__ == '__main__': + main() diff --git a/mir_restapi/package.xml b/mir_restapi/package.xml new file mode 100644 index 00000000..24e9b8b7 --- /dev/null +++ b/mir_restapi/package.xml @@ -0,0 +1,23 @@ + + + + mir_restapi + 0.0.0 + TODO: Package description + niemann + TODO: License declaration + + rclpy + std_msgs + std_srvs + rcl_interfaces + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/mir_restapi/resource/mir_restapi b/mir_restapi/resource/mir_restapi new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/mir_restapi/resource/mir_restapi @@ -0,0 +1 @@ + diff --git a/mir_restapi/setup.cfg b/mir_restapi/setup.cfg new file mode 100644 index 00000000..22a79fde --- /dev/null +++ b/mir_restapi/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/mir_restapi +[install] +install_scripts=$base/lib/mir_restapi diff --git a/mir_restapi/setup.py b/mir_restapi/setup.py new file mode 100644 index 00000000..c9f0853c --- /dev/null +++ b/mir_restapi/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'mir_restapi' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*launch.py'))], + install_requires=['setuptools'], + zip_safe=True, + maintainer='niemann', + maintainer_email='soenke.niemann@ipk.fraunhofer.de', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'mir_restapi_server = mir_restapi.mir_restapi_server:main', + 'mir_restapi_sync_time = mir_restapi.mir_restapi_sync_time:main' + ], + }, +) diff --git a/mir_restapi/test/test_copyright.py b/mir_restapi/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/mir_restapi/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/mir_restapi/test/test_flake8.py b/mir_restapi/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/mir_restapi/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/mir_restapi/test/test_pep257.py b/mir_restapi/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/mir_restapi/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/mir_robot/CMakeLists.txt b/mir_robot/CMakeLists.txt index 559abf94..78ec3509 100644 --- a/mir_robot/CMakeLists.txt +++ b/mir_robot/CMakeLists.txt @@ -1,4 +1,4 @@ cmake_minimum_required(VERSION 3.5.1) project(mir_robot) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) +ament_package() \ No newline at end of file diff --git a/mir_robot/package.xml b/mir_robot/package.xml index 7eba3ffb..e46b302b 100644 --- a/mir_robot/package.xml +++ b/mir_robot/package.xml @@ -1,5 +1,5 @@ - + mir_robot 1.1.3 @@ -15,18 +15,21 @@ https://github.com/dfki-ric/mir_robot https://github.com/dfki-ric/mir_robot/issues - catkin + ament_cmake + mir_description + mir_gazebo + - + ament_cmake + diff --git a/ros2.repos b/ros2.repos new file mode 100644 index 00000000..0d7a22fe --- /dev/null +++ b/ros2.repos @@ -0,0 +1,13 @@ +repositories: + ira_laser_tools: + type: git + url: https://github.com/relffok/ira_laser_tools.git + version: galactic-devel + rclpy_message_converter: + type: git + url: https://github.com/relffok/rospy_message_converter.git + version: foxy-devel + twist_stamper: + type: git + url: https://github.com/joshnewans/twist_stamper.git + version: 32e0eed68c76d9d3fa64b54e8313aa9a6d6bd99d diff --git a/sdc21x0/CHANGELOG.rst b/sdc21x0/CHANGELOG.rst deleted file mode 100644 index 6131f3ac..00000000 --- a/sdc21x0/CHANGELOG.rst +++ /dev/null @@ -1,46 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package sdc21x0 -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.3 (2021-06-11) ------------------- - -1.1.2 (2021-05-12) ------------------- - -1.1.1 (2021-02-11) ------------------- -* Contributors: Martin Günther - -1.1.0 (2020-06-30) ------------------- -* Initial release into noetic -* Contributors: Martin Günther - -1.0.6 (2020-06-30) ------------------- -* Set cmake_policy CMP0048 to fix warning -* Contributors: Martin Günther - -1.0.5 (2020-05-01) ------------------- -* Add sdc21x0 package, MC/currents topic -* Contributors: Martin Günther - -* Add sdc21x0 package, MC/currents topic -* Contributors: Martin Günther - -1.0.4 (2019-05-06) ------------------- - -1.0.3 (2019-03-04) ------------------- - -1.0.2 (2018-07-30) ------------------- - -1.0.1 (2018-07-17) ------------------- - -1.0.0 (2018-07-12) ------------------- diff --git a/sdc21x0/CMakeLists.txt b/sdc21x0/CMakeLists.txt deleted file mode 100644 index b5d92859..00000000 --- a/sdc21x0/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.5.1) -cmake_policy(SET CMP0048 NEW) -project(sdc21x0) - -find_package(catkin REQUIRED COMPONENTS - message_generation - std_msgs -) - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -# Generate messages in the 'msg' folder -add_message_files( - FILES - Encoders.msg - MotorCurrents.msg - StampedEncoders.msg -) - -# Generate services in the 'srv' folder -add_service_files( - FILES - Flags.srv -) - -# Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs -) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - CATKIN_DEPENDS - message_runtime - std_msgs -) diff --git a/sdc21x0/msg/Encoders.msg b/sdc21x0/msg/Encoders.msg deleted file mode 100644 index 9fff122e..00000000 --- a/sdc21x0/msg/Encoders.msg +++ /dev/null @@ -1,3 +0,0 @@ -float32 time_delta # Time since last encoder update. -int32 left_wheel # Encoder counts (absolute or relative) -int32 right_wheel # Encoder counts (absolute or relative) diff --git a/sdc21x0/msg/MotorCurrents.msg b/sdc21x0/msg/MotorCurrents.msg deleted file mode 100644 index 233a16bb..00000000 --- a/sdc21x0/msg/MotorCurrents.msg +++ /dev/null @@ -1,2 +0,0 @@ -float32 left_motor -float32 right_motor diff --git a/sdc21x0/msg/StampedEncoders.msg b/sdc21x0/msg/StampedEncoders.msg deleted file mode 100644 index ff8193c9..00000000 --- a/sdc21x0/msg/StampedEncoders.msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -Encoders encoders diff --git a/sdc21x0/package.xml b/sdc21x0/package.xml deleted file mode 100644 index 88c8cae6..00000000 --- a/sdc21x0/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - sdc21x0 - 1.1.3 - Message definitions for the sdc21x0 motor controller - - Martin Günther - Martin Günther - - BSD - - https://github.com/dfki-ric/mir_robot - https://github.com/dfki-ric/mir_robot - https://github.com/dfki-ric/mir_robot/issues - - catkin - - std_msgs - - message_generation - message_runtime - diff --git a/sdc21x0/srv/Flags.srv b/sdc21x0/srv/Flags.srv deleted file mode 100644 index 576a7376..00000000 --- a/sdc21x0/srv/Flags.srv +++ /dev/null @@ -1,3 +0,0 @@ -int32 digitalPort ---- -bool response