diff --git a/.gitignore b/.gitignore index d5dca0c..cbd4eda 100644 --- a/.gitignore +++ b/.gitignore @@ -412,3 +412,4 @@ FodyWeavers.xsd # Built Visual Studio Code Extensions *.vsix +build \ No newline at end of file diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..0dafb17 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,24 @@ +[submodule "src/Libraries/common_msgs"] + path = src/Libraries/common_msgs + url = https://git.pnkr.asia/DuongTD/common_msgs.git +[submodule "src/Libraries/costmap_2d"] + path = src/Libraries/costmap_2d + url = https://git.pnkr.asia/DuongTD/costmap_2d.git +[submodule "src/Libraries/geometry2"] + path = src/Libraries/geometry2 + url = https://git.pnkr.asia/DuongTD/geometry2.git +[submodule "src/Libraries/xmlrpcpp"] + path = src/Libraries/xmlrpcpp + url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git +[submodule "src/Libraries/laser_geometry"] + path = src/Libraries/laser_geometry + url = https://git.pnkr.asia/DuongTD/laser_geometry.git +[submodule "src/Libraries/robot_time"] + path = src/Libraries/robot_time + url = https://git.pnkr.asia/DuongTD/robot_time.git +[submodule "src/Libraries/voxel_grid"] + path = src/Libraries/voxel_grid + url = https://git.pnkr.asia/DuongTD/voxel_grid.git +[submodule "src/Libraries/data_convert"] + path = src/Libraries/data_convert + url = https://git.pnkr.asia/DuongTD/data_convert.git diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..8fba0f3 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,71 @@ +{ + "files.associations": { + "stdexcept": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "ranges": "cpp", + "sstream": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp", + "codecvt": "cpp" + } +} \ No newline at end of file diff --git a/CATKIN_BUILD_README.md b/CATKIN_BUILD_README.md new file mode 100644 index 0000000..47175b9 --- /dev/null +++ b/CATKIN_BUILD_README.md @@ -0,0 +1,58 @@ +# Catkin Build Instructions + +## Vấn đề + +Thư mục này có `CMakeLists.txt` ở root để hỗ trợ **standalone CMake build**. +Tuy nhiên, **catkin workspace không được phép có CMakeLists.txt ở root**. + +Khi chạy `catkin_make` trong thư mục này, bạn sẽ gặp lỗi: +``` +The specified base path contains a CMakeLists.txt but "catkin_make" must be invoked in the root of workspace +``` + +## Giải pháp + +### Option 1: Sử dụng script tự động (Khuyến nghị) + +```bash +# Chạy script setup +./setup_catkin_workspace.sh + +# Build trong workspace mới +cd ../pnkx_nav_catkin_ws +catkin_make +source devel/setup.bash +``` + +### Option 2: Tạo workspace thủ công + +```bash +# Tạo workspace +mkdir -p ~/pnkx_nav_catkin_ws/src +cd ~/pnkx_nav_catkin_ws/src + +# Link các packages có package.xml +ln -s /path/to/pnkx_nav_core/src/Navigations/Packages/move_base move_base +# ... link các packages khác nếu cần + +# Build +cd ~/pnkx_nav_catkin_ws +catkin_make +``` + +## Build Systems + +Project này hỗ trợ **2 build systems**: + +1. **Standalone CMake** (mặc định) + - Build trực tiếp trong thư mục này: `mkdir build && cd build && cmake .. && make` + - Không cần catkin workspace + +2. **Catkin** (cho ROS integration) + - Cần tạo workspace riêng (xem hướng dẫn trên) + - Packages tự động detect catkin mode khi build trong workspace + +## Chi tiết + +Xem `src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md` để biết chi tiết về cách build package `move_base` với cả hai hệ thống. + diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..1d6da81 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,140 @@ +# ======================================================== +# ROOT CMakeLists.txt +# Đặt ở: Navigations/CMakeLists.txt +# ======================================================== +cmake_minimum_required(VERSION 3.0.2) +project(navigations VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# Flags chung +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +message(STATUS "========================================") +message(STATUS "Building Navigations Project") +message(STATUS "Build type: ${CMAKE_BUILD_TYPE}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "========================================") + +# Build các packages theo thứ tự phụ thuộc +# 1. Core libraries (header-only hoặc base libraries) + +if (NOT TARGET tf3) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/tf3) +endif() + +if (NOT TARGET robot_time) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_time) +endif() + +if (NOT TARGET xmlrpcpp) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/xmlrpcpp) +endif() + +if (NOT TARGET robot_cpp) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_cpp) +endif() + +if (NOT TARGET data_convert) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/data_convert) +endif() + +if (NOT TARGET geometry2) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/geometry2) +endif() + +if (NOT TARGET common_msgs) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/common_msgs) +endif() + +if (NOT TARGET laser_geometry) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_geometry) +endif() + +if (NOT TARGET voxel_grid) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/voxel_grid) +endif() + +if (NOT TARGET nav_grid) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid) +endif() + +if (NOT TARGET nav_2d_msgs) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_msgs) +endif() + +if (NOT TARGET costmap_2d) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/costmap_2d) +endif() + +if (NOT TARGET nav_2d_utils) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_utils) +endif() + +if (NOT TARGET nav_core) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core) +endif() + +if (NOT TARGET nav_core2) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2) +endif() + + +if (NOT TARGET nav_core_adapter) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter) +endif() + +if (NOT TARGET move_base_core) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/move_base_core) +endif() + +if (NOT TARGET mkt_msgs) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_msgs) +endif() + +if (NOT TARGET angles) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/angles) +endif() + +if (NOT TARGET kalman) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/kalman) +endif() + +if (NOT TARGET score_algorithm) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm) +endif() + +#if (NOT TARGET mkt_algorithm) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm) +#endif() + +if (NOT TARGET two_points_planner) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner) +endif() + + +# 2. Main packages (phụ thuộc vào cores) +message(STATUS "[move_base] Shared library configured") +if (NOT TARGET move_base) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base) +endif() + +# C API for .NET/C# integration +if (NOT TARGET navigation_c_api) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api) +endif() + +message(STATUS "========================================") +message(STATUS "All packages configured successfully") +message(STATUS "========================================") + + diff --git a/config/base_local_planner_params.yaml b/config/base_local_planner_params.yaml new file mode 100644 index 0000000..853cace --- /dev/null +++ b/config/base_local_planner_params.yaml @@ -0,0 +1,48 @@ +#For full documentation of the parameters in this file, and a list of all the +#parameters available for TrajectoryPlannerROS, please see +#http://www.ros.org/wiki/base_local_planner +TrajectoryPlannerROS: + #Set the acceleration limits of the robot + acc_lim_th: 3.2 + acc_lim_x: 0.5 + acc_lim_y: 0.5 + + #Set the velocity limits of the robot + max_vel_x: 0.4 + min_vel_x: 0.1 + max_rotational_vel: 1.0 + min_in_place_rotational_vel: 0.4 + + #The velocity the robot will command when trying to escape from a stuck situation + escape_vel: -0.2 + + #For this example, we'll use a holonomic robot + holonomic_robot: false + + #Since we're using a holonomic robot, we'll set the set of y velocities it will sample + y_vels: [-0.3, -0.1, 0.1, 0.3] + + #Set the tolerance on achieving a goal + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.05 + + #We'll configure how long and with what granularity we'll forward simulate trajectories + sim_time: 3.0 + sim_granularity: 0.025 + vx_samples: 5 + vtheta_samples: 20 + + #Parameters for scoring trajectories + goal_distance_bias: 0.4 + path_distance_bias: 0.7 + occdist_scale: 0.3 + heading_lookahead: 0.325 + + #We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example + dwa: false + + #How far the robot must travel before oscillation flags are reset + oscillation_reset_dist: 0.01 + + #Eat up the plan as the robot moves along it + prune_plan: false \ No newline at end of file diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml new file mode 100755 index 0000000..af52a10 --- /dev/null +++ b/config/costmap_common_params.yaml @@ -0,0 +1,54 @@ +robot_base_frame: base_footprint +transform_tolerance: 1.0 +obstacle_range: 3.0 +#mark_threshold: 1 +publish_voxel_map: true +footprint_padding: 0.0 + +navigation_map: + map_topic: /map + map_pkg: managerments + map_file: maze + +virtual_walls_map: + map_topic: /virtual_walls/map + namespace: /virtual_walls + map_pkg: managerments + map_file: maze + use_maximum: true + lethal_cost_threshold: 100 + +obstacles: + observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing + f_scan_marking: + topic: /f_scan + data_type: LaserScan + clearing: false + marking: true + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + f_scan_clearing: + topic: /f_scan + data_type: LaserScan + clearing: true + marking: false + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + b_scan_marking: + topic: /b_scan + data_type: LaserScan + clearing: false + marking: true + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + b_scan_clearing: + topic: /b_scan + data_type: LaserScan + clearing: true + marking: false + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 diff --git a/config/costmap_global_params.yaml b/config/costmap_global_params.yaml new file mode 100755 index 0000000..ae6d769 --- /dev/null +++ b/config/costmap_global_params.yaml @@ -0,0 +1,13 @@ +global_costmap: + global_frame: map + update_frequency: 1.0 + publish_frequency: 1.0 + raytrace_range: 2.0 + resolution: 0.05 + z_resolution: 0.2 + rolling_window: false + z_voxels: 10 + 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. + diff --git a/config/costmap_global_params_plugins_no_virtual_walls.yaml b/config/costmap_global_params_plugins_no_virtual_walls.yaml new file mode 100755 index 0000000..06d0037 --- /dev/null +++ b/config/costmap_global_params_plugins_no_virtual_walls.yaml @@ -0,0 +1,10 @@ +global_costmap: + frame_id: map + plugins: + - {name: navigation_map, type: "costmap_2d::StaticLayer" } + - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" } + - {name: obstacles, type: "costmap_2d::VoxelLayer" } + - {name: inflation, type: "costmap_2d::InflationLayer" } + obstacles: + enabled: false + footprint_clearing_enabled: false \ No newline at end of file diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml new file mode 100755 index 0000000..144ee12 --- /dev/null +++ b/config/costmap_local_params.yaml @@ -0,0 +1,16 @@ +local_costmap: + global_frame: odom + update_frequency: 6.0 + publish_frequency: 6.0 + rolling_window: true + raytrace_range: 2.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.3 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. + width: 8.0 + height: 8.0 + origin_x: 0.0 + origin_y: 0.0 \ No newline at end of file diff --git a/config/costmap_local_params_plugins_no_virtual_walls.yaml b/config/costmap_local_params_plugins_no_virtual_walls.yaml new file mode 100755 index 0000000..6ef480b --- /dev/null +++ b/config/costmap_local_params_plugins_no_virtual_walls.yaml @@ -0,0 +1,8 @@ +local_costmap: + frame_id: odom + plugins: + - {name: obstacles, type: "costmap_2d::VoxelLayer" } + - {name: inflation, type: "costmap_2d::InflationLayer" } + obstacles: + enabled: true + footprint_clearing_enabled: true diff --git a/config/custom_global_params.yaml b/config/custom_global_params.yaml new file mode 100644 index 0000000..6536a6e --- /dev/null +++ b/config/custom_global_params.yaml @@ -0,0 +1,17 @@ +base_global_planner: CustomPlanner +CustomPlanner: + environment_type: XYThetaLattice + planner_type: ARAPlanner + allocated_time: 10.0 + initial_epsilon: 1.0 + force_scratch_limit: 10000 + forward_search: false + nominalvel_mpersecs: 0.8 + timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s + allow_unknown: true + directory_to_save_paths: "/init/paths" + pathway_filename: "pathway.txt" + current_pose_topic_name: "/amcl_pose" + map_frame_id: "map" + base_frame_id: "base_link" + diff --git a/config/dwa_local_planner_params.yaml b/config/dwa_local_planner_params.yaml new file mode 100755 index 0000000..b8d766a --- /dev/null +++ b/config/dwa_local_planner_params.yaml @@ -0,0 +1,55 @@ +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/config/ekf.yaml b/config/ekf.yaml new file mode 100755 index 0000000..7344ee3 --- /dev/null +++ b/config/ekf.yaml @@ -0,0 +1,217 @@ +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. +frequency: 50 + +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. +sensor_timeout: 0.1 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. +two_d_mode: true + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. +transform_time_offset: 0.0 + +# Use this parameter to specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. +transform_timeout: 0.0 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. +print_diagnostics: true + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. +debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. +debug_out_file: /path/to/debug/file.txt + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. +publish_tf: true + +# Whether to publish the acceleration state. Defaults to false if unspecified. +publish_acceleration: false + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state +# estimation node from robot_localization! However, that instance should *not* fuse the global data. +map_frame: map # Defaults to "map" if unspecified +odom_frame: $(arg tf_prefix)odom # Defaults to "odom" if unspecified +base_link_frame: $(arg tf_prefix)base_footprint # Defaults to "base_link" if unspecified +world_frame: $(arg tf_prefix)odom # Defaults to the value of odom_frame if unspecified + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. +odom0: odom + +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. +# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html +odom0_config: [false, false, false, # x y z + false, false, false, # roll pitch yaw + true, true, false, # vx vy vz + false, false, true, # vroll vpitch vyaw + false, false, false] # ax ay az + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. +odom0_queue_size: 10 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. +odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. +odom0_differential: false + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. +odom0_relative: false + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. +#odom0_pose_rejection_threshold: 5 +#odom0_twist_rejection_threshold: 1 + +# Further input parameter examples +# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html +imu0: imu_data +imu0_config: [false, false, false, # x y z + false, false, true, # roll pitch yaw + false, false, false, # vx vy vz + false, false, true, # vroll vpitch vyaw + true, false, false] # ax ay az +imu0_nodelay: false +imu0_differential: false +imu0_relative: true +imu0_queue_size: 10 +#imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names +#imu0_twist_rejection_threshold: 0.8 # +#imu0_linear_acceleration_rejection_threshold: 0.8 # + +# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set +# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. +imu0_remove_gravitational_acceleration: false + +# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no +# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During +# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be +# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When +# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially +# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance +# for the velocity variable in question, or decrease the variance of the variable in question in the measurement +# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we +# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during +# predicition. Note that if an acceleration measurement for the variable in question is available from one of the +# inputs, the control term will be ignored. +# Whether or not we use the control input during predicition. Defaults to false. +use_control: false +# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to +# false. +stamped_control: false +# The last issued control command will be used in prediction for this period. Defaults to 0.2. +control_timeout: 0.2 +# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. +control_config: [true, false, false, false, false, true] +# Places limits on how large the acceleration term will be. Should match your robot's kinematics. +acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +# Acceleration and deceleration limits are not always the same for robots. +deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these +# gains +acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these +# gains +deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + +# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is +# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each +# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. +# However, if users find that a given variable is slow to converge, one approach is to increase the +# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error +# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are +# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if +# unspecified. +process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + +# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal +# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in +# question. Users should take care not to use large values for variables that will not be measured directly. The values +# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below +#if unspecified. +initial_estimate_covariance: [100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] diff --git a/config/localization.yaml b/config/localization.yaml new file mode 100644 index 0000000..677cb96 --- /dev/null +++ b/config/localization.yaml @@ -0,0 +1,36 @@ +Amcl: + use_map_topic: true + odom_model_type: "diff-corrected" + gui_publish_rate: 5.0 + save_pose_rate: 0.5 + laser_max_beams: 300 + laser_min_range: -1.0 + laser_max_range: -1.0 + min_particles: 1000 + max_particles: 3000 + kld_err: 0.05 + kld_z: 0.99 + odom_alpha1: 0.02 + odom_alpha2: 0.01 + odom_alpha3: 0.01 + odom_alpha4: 0.02 + laser_z_hit: 0.5 + laser_z_short: 0.05 + laser_z_max: 0.05 + laser_z_rand: 0.5 + laser_sigma_hit: 0.2 + laser_lambda_short: 0.1 + laser_model_type: "likelihood_field" + laser_likelihood_max_dist: 1.0 + update_min_d: 0.05 + update_min_a: 0.05 + odom_frame_id: odom + base_frame_id: base_footprint + global_frame_id: map + resample_interval: 1 + transform_tolerance: 0.2 + recovery_alpha_slow: 0.001 + recovery_alpha_fast: 0.001 + initial_pose_x: 0.0 + initial_pose_y: 0.0 + initial_pose_a: 0.0 diff --git a/config/maker_sources.yaml b/config/maker_sources.yaml new file mode 100644 index 0000000..fd4bce8 --- /dev/null +++ b/config/maker_sources.yaml @@ -0,0 +1,152 @@ +maker_sources: trolley charger dock_station undock_station dock_station_2 undock_station_2 +trolley: + plugins: + - {name: 4legs, docking_planner: "DockPlanner", docking_nav: ""} + # - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" } + + 4legs: + maker_goal_frame: trolley_goal + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 1.5 # Cấm sửa không là không chạy được + timeout: 60.0 + vel_x: 0.25 + vel_theta: 0.3 + yaw_goal_tolerance: 0.015 + xy_goal_tolerance: 0.015 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.16 + + qrcode: + maker_goal_frame: qr_trolley + delay: 1.5 # Cấm sửa không là không chạy được + timeout: 60.0 + vel_x: 0.05 + vel_theta: 0.5 + allow_rotate: true + yaw_goal_tolerance: 0.015 + xy_goal_tolerance: 0.02 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.16 + +charger: + plugins: + - {name: charger, docking_planner: "DockPlanner", docking_nav: ""} + + charger: + maker_goal_frame: charger_goal + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 1.5 # Cấm sửa không là không chạy được + timeout: 60 + vel_x: 0.1 + yaw_goal_tolerance: 0.015 + xy_goal_tolerance: 0.015 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.16 + +dock_station: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + + station: + maker_goal_frame: dock_station_goal + footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]] + delay: 1.5 # Cấm sửa không là không chạy được + timeout: 60 + vel_x: 0.15 + vel_theta: 0.3 + yaw_goal_tolerance: 0.015 + xy_goal_tolerance: 0.015 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.4 + +dock_station_2: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + + station: + maker_goal_frame: dock_station_goal_2 + footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]] + delay: 1.5 # Cấm sửa không là không chạy được + timeout: 60 + vel_x: 0.15 + vel_theta: 0.3 + yaw_goal_tolerance: 0.015 + xy_goal_tolerance: 0.015 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.4 + +undock_station: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" } + + station: + maker_goal_frame: undock_station_goal + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 1.5 # Cấm sửa không là không chạy được + timeout: 60.0 + vel_x: 0.25 + vel_theta: 0.3 + yaw_goal_tolerance: 0.015 + xy_goal_tolerance: 0.015 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.16 + + qrcode: + maker_goal_frame: qr_trolley + delay: 1.5 # Cấm sửa không là không chạy được + timeout: 60.0 + vel_x: 0.05 + vel_theta: 0.5 + allow_rotate: true + yaw_goal_tolerance: 0.015 + xy_goal_tolerance: 0.03 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.16 + +undock_station_2: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" } + + station: + maker_goal_frame: undock_station_goal_2 + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 1.5 # Cấm sửa không là không chạy được + timeout: 60.0 + vel_x: 0.25 + vel_theta: 0.5 + yaw_goal_tolerance: 0.015 + xy_goal_tolerance: 0.03 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.16 + + qrcode: + maker_goal_frame: qr_trolley + delay: 1.5 # Cấm sửa không là không chạy được + timeout: 60.0 + vel_x: 0.05 + vel_theta: 0.5 + allow_rotate: true + yaw_goal_tolerance: 0.02 + xy_goal_tolerance: 0.02 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.16 \ No newline at end of file diff --git a/config/mapping.yaml b/config/mapping.yaml new file mode 100644 index 0000000..e58fce3 --- /dev/null +++ b/config/mapping.yaml @@ -0,0 +1,63 @@ +SlamToolBox: + # 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_link + scan_topic: /scan + mode: mapping #localization + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 10.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 14400. + 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 + 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/config/move_base_common_params.yaml b/config/move_base_common_params.yaml new file mode 100755 index 0000000..871aa70 --- /dev/null +++ b/config/move_base_common_params.yaml @@ -0,0 +1,24 @@ + +### replanning +controller_frequency: 20.0 # run controller at 15.0 Hz +controller_patience: 0.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: 5 # ... or after 10 attempts (whichever happens first) +oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s +oscillation_distance: 0.4 +### recovery behaviors +recovery_behavior_enabled: true +recovery_behaviors: [ + {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, + {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, + ] + +conservative_reset: + reset_distance: 3.0 # clear obstacles farther away than 3.0 m + invert_area_to_clear: true + +aggressive_reset: + reset_distance: 3.0 + + diff --git a/config/mpc_local_planner_params.yaml b/config/mpc_local_planner_params.yaml new file mode 100755 index 0000000..aa6a592 --- /dev/null +++ b/config/mpc_local_planner_params.yaml @@ -0,0 +1,106 @@ +base_local_planner: mpc_local_planner/MpcLocalPlannerROS +MpcLocalPlannerROS: + + odom_topic: odom + + ## Robot settings + robot: + type: "unicycle" + unicycle: + max_vel_x: 0.5 + max_vel_x_backwards: 0.3 + max_vel_theta: 0.3 + acc_lim_x: 0.4 # deactive bounds with zero + dec_lim_x: 0.4 # deactive bounds with zero + acc_lim_theta: 0.6 # deactivate bounds with zero + + ## Footprint model for collision avoidance + footprint_model: + type: "point" + is_footprint_dynamic: False + + ## Collision avoidance + collision_avoidance: + min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model + enable_dynamic_obstacles: False + force_inclusion_dist: 0.5 + cutoff_dist: 2.0 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 1.5 + collision_check_no_poses: 5 + + ## Planning grid + grid: + type: "fd_grid" + grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist + dt_ref: 0.3 # and here the corresponding temporal resolution + xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below + warm_start: True + collocation_method: "forward_differences" + cost_integration_method: "left_sum" + variable_grid: + enable: False # We want a fixed grid + min_dt: 0.0; + max_dt: 10.0; + grid_adaptation: + enable: True + dt_hyst_ratio: 0.1 + min_grid_size: 2 + max_grid_size: 50 + + ## Planning options + planning: + objective: + type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly + quadratic_form: + state_weights: [2.0, 2.0, 0.25] + control_weights: [0.1, 0.05] + integral_form: False + terminal_cost: + type: "quadratic" # can be "none" + quadratic: + final_state_weights: [10.0, 10.0, 0.5] + terminal_constraint: + type: "none" # can be "none" + l2_ball: + weight_matrix: [1.0, 1.0, 1.0] + radius: 5 + + ## Controller options + controller: + outer_ocp_iterations: 1 + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.04 + global_plan_overwrite_orientation: False + global_plan_prune_distance: 1.5 + allow_init_with_backward_motion: True + max_global_plan_lookahead_dist: 1.0 # Check horizon length + force_reinit_new_goal_dist: 1.0 + force_reinit_new_goal_angular: 1.57 + force_reinit_num_steps: 0 + prefer_x_feedback: False + publish_ocp_results: True + + ## Solver settings + solver: + type: "ipopt" + ipopt: + iterations: 100 + max_cpu_time: -1.0 + ipopt_numeric_options: + tol: 1e-3 + ipopt_string_options: + linear_solver: "mumps" + hessian_approximation: "exact" # exact or limited-memory + lsq_lm: + iterations: 10 + weight_init_eq: 2 + weight_init_ineq: 2 + weight_init_bounds: 2 + weight_adapt_factor_eq: 1.5 + weight_adapt_factor_ineq: 1.5 + weight_adapt_factor_bounds: 1.5 + weight_adapt_max_eq: 500 + weight_adapt_max_ineq: 500 + weight_adapt_max_bounds: 500 + diff --git a/config/mprim/genmprim_unicycle_highcost_5cm.m b/config/mprim/genmprim_unicycle_highcost_5cm.m new file mode 100755 index 0000000..2810593 --- /dev/null +++ b/config/mprim/genmprim_unicycle_highcost_5cm.m @@ -0,0 +1,280 @@ +% /* +% * 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/config/mprim/genmprim_unicycle_highcost_5cm.py b/config/mprim/genmprim_unicycle_highcost_5cm.py new file mode 100755 index 0000000..c8801c0 --- /dev/null +++ b/config/mprim/genmprim_unicycle_highcost_5cm.py @@ -0,0 +1,416 @@ +#!/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, pause, axis, sin, pinv, 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 + + # commented out because basemprimendpts33p75_c is undefined + # 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') + + # commented out because basemprimendpts11p25_c is undefined + # 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') + + # commented out because basemprimendpts33p75_c is undefined + # 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') + + # commented out because basemprimendpts11p25_c is undefined + # 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])) + # 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/config/mprim/unicycle_5cm.mprim b/config/mprim/unicycle_5cm.mprim new file mode 100755 index 0000000..cb56cd0 --- /dev/null +++ b/config/mprim/unicycle_5cm.mprim @@ -0,0 +1,1203 @@ +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/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim b/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim new file mode 100755 index 0000000..f27907b --- /dev/null +++ b/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim @@ -0,0 +1,1683 @@ +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/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim b/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim new file mode 100755 index 0000000..0f11e5d --- /dev/null +++ b/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim @@ -0,0 +1,1683 @@ +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/config/mprim/unicycle_5cm_noreverse_trolley.mprim b/config/mprim/unicycle_5cm_noreverse_trolley.mprim new file mode 100755 index 0000000..596f5c5 --- /dev/null +++ b/config/mprim/unicycle_5cm_noreverse_trolley.mprim @@ -0,0 +1,2403 @@ +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/config/mprim/unicycle_highcost_10cm.mprim b/config/mprim/unicycle_highcost_10cm.mprim new file mode 100755 index 0000000..de16976 --- /dev/null +++ b/config/mprim/unicycle_highcost_10cm.mprim @@ -0,0 +1,1683 @@ +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/config/mprim/unicycle_highcost_1cm.mprim b/config/mprim/unicycle_highcost_1cm.mprim new file mode 100755 index 0000000..aa9670d --- /dev/null +++ b/config/mprim/unicycle_highcost_1cm.mprim @@ -0,0 +1,1683 @@ +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/config/mprim/unicycle_highcost_2_5cm.mprim b/config/mprim/unicycle_highcost_2_5cm.mprim new file mode 100755 index 0000000..6bf962e --- /dev/null +++ b/config/mprim/unicycle_highcost_2_5cm.mprim @@ -0,0 +1,1683 @@ +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/config/mprim/unicycle_highcost_2cm.mprim b/config/mprim/unicycle_highcost_2cm.mprim new file mode 100755 index 0000000..b87c310 --- /dev/null +++ b/config/mprim/unicycle_highcost_2cm.mprim @@ -0,0 +1,1683 @@ +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/config/mprim/unicycle_highcost_5cm.mprim b/config/mprim/unicycle_highcost_5cm.mprim new file mode 100755 index 0000000..bff2688 --- /dev/null +++ b/config/mprim/unicycle_highcost_5cm.mprim @@ -0,0 +1,1683 @@ +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/config/mqtt_general.yaml b/config/mqtt_general.yaml new file mode 100644 index 0000000..1230673 --- /dev/null +++ b/config/mqtt_general.yaml @@ -0,0 +1,8 @@ +MQTT: + Name: T800 + Host: 172.20.235.170 + Port: 1885 + Client_ID: T800 + Username: robotics + Password: robotics + Keep_Alive: 60 \ No newline at end of file diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml new file mode 100644 index 0000000..5b76e45 --- /dev/null +++ b/config/pnkx_local_planner_params.yaml @@ -0,0 +1,186 @@ + +position_planner_name: PNKXLocalPlanner +docking_planner_name: PNKXDockingLocalPlanner +go_straight_planner_name: PNKXGoStraightLocalPlanner +rotate_planner_name: PNKXRotateLocalPlanner + +base_local_planner: nav_core_adapter::LocalPlannerAdapter +yaw_goal_tolerance: 0.017 +xy_goal_tolerance: 0.03 +min_approach_linear_velocity: 0.06 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) +stateful: true +publish_topic: true + +LocalPlannerAdapter: + PNKXLocalPlanner: + # Algorithm + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory + algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + # Goal checking + goal_checker_name: mkt_plugins::GoalChecker + + PNKXDockingLocalPlanner: + # Algorithm + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory + algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + # Goal checking + goal_checker_name: mkt_plugins::GoalChecker + + PNKXGoStraightLocalPlanner: + # Algorithm + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_nav_name: mkt_algorithm::diff::GoStraight + # Goal checking + goal_checker_name: mkt_plugins::GoalChecker + + PNKXRotateLocalPlanner: + # Algorithm + algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + # Goal checking + goal_checker_name: mkt_plugins::SimpleGoalChecker + stateful: true + +LimitedAccelGenerator: + max_vel_x: 1.2 + min_vel_x: -1.2 + + max_vel_y: 0.0 # diff drive robot + min_vel_y: 0.0 # diff drive robot + + max_speed_xy: 2.0 # 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: 0.9 # max_rot_vel: 1.0 # choose slightly less than the base's capability + min_vel_theta: 0.04 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity + + acc_lim_x: 1.0 + acc_lim_y: 0.0 # diff drive robot + acc_lim_theta: 0.9 + decel_lim_x: -1.0 + decel_lim_y: -0.0 + decel_lim_theta: -1.5 + + # Whether to split the path into segments or not + split_path: true + sim_time: 1.5 + vx_samples: 15 + vy_samples: 1 + vtheta_samples: 10 + discretize_by_time: true + angular_granularity: 0.05 + linear_granularity: 0.1 + # sim_period + include_last_point: true + +PredictiveTrajectory: + avoid_obstacles: false + xy_local_goal_tolerance: 0.01 + angle_threshold: 0.6 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + # only when true: + min_lookahead_dist: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3) + max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9) + lookahead_time: 1.6 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2) + + # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + angular_decel_zone: 0.1 + + # stoped + rot_stopped_velocity: 0.1 + trans_stopped_velocity: 0.1 + + # Regulated linear velocity scaling + use_regulated_linear_velocity_scaling: false # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true) + # only when true: + regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9) + regulated_linear_scaling_min_speed: 0.15 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25) + + # Inflation cost scaling (Limit velocity by proximity to obstacles) + use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true) + inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0 + cost_scaling_dist: 0.2 # (default: 0.6) + cost_scaling_gain: 2.0 # (default: 1.0) + +GoStraight: + avoid_obstacles: false + xy_local_goal_tolerance: 0.01 + angle_threshold: 0.6 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + # only when true: + min_lookahead_dist: 0.8 # The minimum lookahead distance (m) threshold. (default: 0.3) + max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) + lookahead_time: 2.5 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.6 # Minimum squared journey to consider for goal (default: 0.2) + + # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.35 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + + # stoped + rot_stopped_velocity: 0.03 + trans_stopped_velocity: 0.06 + + use_regulated_linear_velocity_scaling: false + use_cost_regulated_linear_velocity_scaling: false + +RotateToGoal: + avoid_obstacles: false + xy_local_goal_tolerance: 0.01 + angle_threshold: 0.6 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + # only when true: + min_lookahead_dist: 0.8 # The minimum lookahead distance (m) threshold. (default: 0.3) + max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) + lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.6 # Maximum squared journey to consider for goal (default: 0.2) + + # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + angular_decel_zone: 0.1 + + # stoped + rot_stopped_velocity: 0.03 + trans_stopped_velocity: 0.06 + + # Regulated linear velocity scaling + use_regulated_linear_velocity_scaling: false # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true) + # only when true: + regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9) + regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25) + + # Inflation cost scaling (Limit velocity by proximity to obstacles) + use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true) + inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0 + cost_scaling_dist: 0.2 # (default: 0.6) + cost_scaling_gain: 2.0 # (default: 1.0) \ No newline at end of file diff --git a/config/ros_diff_drive_controller.yaml b/config/ros_diff_drive_controller.yaml new file mode 100755 index 0000000..e498438 --- /dev/null +++ b/config/ros_diff_drive_controller.yaml @@ -0,0 +1,71 @@ +# ----------------------------------- +left_wheel : 'left_wheel_joint' +right_wheel : 'right_wheel_joint' +publish_rate: 50 # this is what the real MiR platform publishes (default: 50) +# 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: true +enable_wheel_tf: true +allow_multiple_cmd_vel_publishers: true +# open_loop: false +# Wheel separation and diameter. These are both optional. +# diff_drive_controller will attempt to read either one or both from the +# URDF if not specified as a parameter. +# We don't set the value here because it's different for each MiR type (100, 250, ...), and +# the plugin figures out the correct values. +wheel_separation : 0.5106 +wheel_radius : 0.1 + +# Wheel separation and radius multipliers +wheel_separation_multiplier: 1.0 # default: 1.0 +wheel_radius_multiplier : 1.0 # default: 1.0 + +# Velocity commands timeout [s], default 0.5 +cmd_vel_timeout: 1.0 + +# frame_ids (same as real MiR platform) +base_frame_id: base_footprint # default: base_link base_footprint +odom_frame_id: odom # default: odom + +# Velocity and acceleration limits +# Whenever a min_* is unspecified, default to -max_* +linear: + x: + has_velocity_limits : true + max_velocity : 1.5 # m/s; move_base max_vel_x: 0.8 + min_velocity : -1.5 # m/s + has_acceleration_limits: true + max_acceleration : 0.5 # m/s^2; move_base acc_lim_x: 1.5 + min_acceleration : -0.5 # m/s^2 + has_jerk_limits : true + max_jerk : 3.0 # m/s^3 +angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s; move_base max_rot_vel: 1.0 + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 3.0 # rad/s^2; move_base acc_lim_th: 2.0 + has_jerk_limits : true + max_jerk : 2.5 # rad/s^3 + +left_wheel_joint: + lookup_name: WheelPlugin + max_publish_rate: 50 + mesurement_topic: left_encoder + frame_id: left_wheel_link + wheel_topic: /left_wheel + subscribe_queue_size: 1 + command_timeout: 5.0 + origin : [0.0, 0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id + +right_wheel_joint: + lookup_name: WheelPlugin + max_publish_rate: 50 + mesurement_topic: right_encoder + frame_id: right_wheel_link + wheel_topic: /right_wheel + subscribe_queue_size: 1 + command_timeout: 5.0 + origin : [0.0, -0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id diff --git a/config/sbpl_global_params.yaml b/config/sbpl_global_params.yaml new file mode 100755 index 0000000..ec2ac75 --- /dev/null +++ b/config/sbpl_global_params.yaml @@ -0,0 +1,10 @@ +base_global_planner: SBPLLatticePlanner +SBPLLatticePlanner: + environment_type: XYThetaLattice + planner_type: ARAPlanner + allocated_time: 10.0 + initial_epsilon: 1.0 + force_scratch_limit: 10000 + forward_search: true + nominalvel_mpersecs: 0.2 + timetoturn45degsinplace_secs: 0.6 # = 0.6 rad/s diff --git a/config/sick_line_guidance_mls.yaml b/config/sick_line_guidance_mls.yaml new file mode 100644 index 0000000..959a137 --- /dev/null +++ b/config/sick_line_guidance_mls.yaml @@ -0,0 +1,61 @@ +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +# +# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats) +# rate: 100 # heartbeat rate (1/rate in seconds) +# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started +nodes: + f_mlse: + id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: mls/SICK-MLS.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"] + # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022 + + # sick_line_guidance configuration of this node: + sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported + sick_topic: "f_mlse" # MLS_Measurement messages are published in topic "/mls" + subscribe_queue_size: 1 + sick_frame_id: "f_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0 + # "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600 + # "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0 + # "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0 + # + + b_mlse: + id: 0x02 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: mls/SICK-MLS.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"] + # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022 + + # sick_line_guidance configuration of this node: + sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported + sick_topic: "b_mlse" # MLS_Measurement messages are published in topic "/mls" + subscribe_queue_size: 1 + sick_frame_id: "b_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0 + # "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600 + # "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0 + # "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0 + # \ No newline at end of file diff --git a/config/two_points_global_params.yaml b/config/two_points_global_params.yaml new file mode 100644 index 0000000..ca0b60f --- /dev/null +++ b/config/two_points_global_params.yaml @@ -0,0 +1,3 @@ +base_global_planner: TwoPointsPlanner +TwoPointsPlanner: + lethal_obstacle: 20 \ No newline at end of file diff --git a/doc/architecture_discussion.md b/doc/architecture_discussion.md new file mode 100644 index 0000000..7de5531 --- /dev/null +++ b/doc/architecture_discussion.md @@ -0,0 +1,446 @@ +# Thảo luận về Kiến trúc Navigation cho AMR + +## Mục tiêu dự án + +Nghiên cứu và phát triển hệ thống navigation (di chuyển bám quỹ đạo) cho robot AMR dạng hai bánh vi sai, **không sử dụng ROS**, viết thuần C++ với CMake trên Linux. + +## Yêu cầu chức năng + +Từ `readme.md`: + +- Có khả năng thay đổi mô hình kinematics (dạng hai bánh vi sai, dạng steering bicycle, ...) +- Có chức năng di chuyển từ điểm khởi đầu đến điểm đích +- Có chức năng xoay tròn +- Có chức năng đi thẳng +- Có chức năng thay đổi vận tốc + +## Sơ đồ kiến trúc hệ thống + +Kiến trúc được thiết kế theo mô hình layered, tách biệt các concerns và cho phép mở rộng dễ dàng: + +```mermaid +flowchart TB + %% ========== API LAYER ========== + subgraph API["🌐 API Layer"] + direction TB + CAPI["📦 C API
━━━━━━━━━━━━━━━━
🔌 nav_c_api
💻 P/Invoke cho .NET/C#
📝 Wrapper Functions
🔗 Interop Layer"] + style API fill:#E3F2FD,stroke:#1976D2,stroke-width:4px,color:#000 + style CAPI fill:#BBDEFB,stroke:#1976D2,stroke-width:3px,font-size:14px + end + + %% ========== USER CONTROLLER LAYER ========== + subgraph UserController["🎮 User Controller Layer"] + direction TB + UserCtrl["🔌 User Controller Plugin
━━━━━━━━━━━━━━━━
📚 Dynamic Loader
⚙️ boost::dll
🎯 Custom Behavior
🔄 Runtime Loading"] + style UserController fill:#F3E5F5,stroke:#7B1FA2,stroke-width:4px,color:#000 + style UserCtrl fill:#E1BEE7,stroke:#7B1FA2,stroke-width:3px,font-size:14px + end + + %% ========== INTERFACE LAYER ========== + subgraph Interface["🔌 Interface Layer"] + direction TB + BaseNav["📋 BaseNavigation
━━━━━━━━━━━━━━━━
🏗️ move_base_core::BaseNavigation
🎯 Abstract Interface
━━━━━━━━━━━━━━━━
📍 moveTo, dockTo
🔄 rotateTo, moveStraightTo
⏸️ pause, resume, cancel
📊 getRobotPose"] + style Interface fill:#FFF3E0,stroke:#E65100,stroke-width:4px,color:#000 + style BaseNav fill:#FFE0B2,stroke:#E65100,stroke-width:3px,font-size:14px + end + + %% ========== MOVE BASE CORE ========== + subgraph MoveBaseCore["⚙️ Move Base Core"] + direction TB + MoveBase["🚀 MoveBase
━━━━━━━━━━━━━━━━
📦 move_base::MoveBase
🔄 State Machine
━━━━━━━━━━━━━━━━
📊 PLANNING
🎮 CONTROLLING
🧹 CLEARING
━━━━━━━━━━━━━━━━
🎛️ Control Loop
🔄 executeCycle"] + style MoveBaseCore fill:#E8F5E9,stroke:#2E7D32,stroke-width:4px,color:#000 + style MoveBase fill:#C8E6C9,stroke:#2E7D32,stroke-width:3px,font-size:14px + end + + %% ========== PLANNING LAYER ========== + subgraph Planning["🗺️ Planning Layer - Plugin System"] + direction LR + GP["🌍 Global Planner
━━━━━━━━━━━━━━━━
📋 nav_core::BaseGlobalPlanner
🔌 Dynamic Plugin
━━━━━━━━━━━━━━━━
🎯 A*, D*, Hybrid A*
📐 Long-range Planning
🗺️ Global Path"] + LP["📍 Local Planner
━━━━━━━━━━━━━━━━
📋 nav_core::BaseLocalPlanner
🔌 Dynamic Plugin
━━━━━━━━━━━━━━━━
🎯 DWA, TEB, MKT
🚗 Velocity Commands
🛡️ Obstacle Avoidance"] + RB["🔄 Recovery Behaviors
━━━━━━━━━━━━━━━━
📋 nav_core::RecoveryBehavior
🔌 Dynamic Plugin
━━━━━━━━━━━━━━━━
🧹 Clear Costmap
🔄 Rotate Recovery
🚨 Emergency Stop"] + style Planning fill:#E1F5FE,stroke:#0277BD,stroke-width:4px,color:#000 + style GP fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px + style LP fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px + style RB fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px + end + + %% ========== COSTMAP LAYER ========== + subgraph Costmap["🗺️ Costmap Layer"] + direction LR + GC["🌍 Global Costmap
━━━━━━━━━━━━━━━━
📦 costmap_2d::Costmap2DROBOT
🌍 frame: map
━━━━━━━━━━━━━━━━
🗺️ Static Map
🚫 Obstacles
💰 Inflation Layer"] + LC["📍 Local Costmap
━━━━━━━━━━━━━━━━
📦 costmap_2d::Costmap2DROBOT
📍 frame: odom
━━━━━━━━━━━━━━━━
🔍 Dynamic Obstacles
📡 Sensor Fusion
⚡ Real-time Updates"] + style Costmap fill:#F1F8E9,stroke:#558B2F,stroke-width:4px,color:#000 + style GC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px + style LC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px + end + + %% ========== ALGORITHMS LAYER ========== + subgraph Algorithms["🧮 Algorithms Layer"] + direction LR + MKTAlgo["🚗 MKT Algorithm
━━━━━━━━━━━━━━━━
⚙️ Diff Drive Kinematics
🚲 Bicycle Kinematics
📐 Trajectory Generation"] + ScoreAlgo["📊 Score Algorithm
━━━━━━━━━━━━━━━━
📈 Trajectory Scoring
✅ Goal Checking
🎯 Path Evaluation"] + KalmanAlgo["🔍 Kalman Filter
━━━━━━━━━━━━━━━━
📊 State Estimation
🔮 Sensor Fusion
📉 Noise Filtering"] + style Algorithms fill:#FCE4EC,stroke:#C2185B,stroke-width:4px,color:#000 + style MKTAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px + style ScoreAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px + style KalmanAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px + end + + %% ========== DATA SOURCES ========== + subgraph DataSources["📡 Data Sources"] + direction TB + Goal["🎯 Goal Input
━━━━━━━━━━━━━━━━
📍 geometry_msgs::PoseStamped
📨 move_base_simple/goal"] + Loc["🌍 Localization
━━━━━━━━━━━━━━━━
📍 Pnkx Loc
🗺️ Global Pose
🔄 Pose Updates"] + TF["🔄 Transform System
━━━━━━━━━━━━━━━━
📐 tf3::BufferCore
🌐 Coordinate Frames
⏱️ Time Synchronization"] + Odom["🚗 Odometry
━━━━━━━━━━━━━━━━
📍 geometry_msgs::Odometry
⚡ Robot Velocity
📊 Position Tracking"] + Laser["📡 Laser Sensors
━━━━━━━━━━━━━━━━
🔍 sensor_msgs::LaserScan
🚫 Obstacle Detection
📏 Distance Measurement"] + Map["🗺️ Map Server
━━━━━━━━━━━━━━━━
📋 nav_msgs::OccupancyGrid
🏗️ Static Map
📐 Map Metadata"] + style DataSources fill:#FFF9C4,stroke:#F57F17,stroke-width:4px,color:#000 + style Goal fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px + style Loc fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px + style TF fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px + style Odom fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px + style Laser fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px + style Map fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px + end + + %% ========== CONTROL LOOP ========== + subgraph ControlLoop["🔄 Control Loop"] + direction LR + CmdVel["⚡ Velocity Command
━━━━━━━━━━━━━━━━
📤 geometry_msgs::Twist
📨 cmd_vel
━━━━━━━━━━━━━━━━
➡️ Linear Velocity
🔄 Angular Velocity"] + BaseCtrl["🎮 Base Controller
━━━━━━━━━━━━━━━━
🚗 diff_driver_controller
🚲 steer_drive_controller
━━━━━━━━━━━━━━━━
⚙️ Kinematics
🔧 Hardware Interface"] + style ControlLoop fill:#FFEBEE,stroke:#C62828,stroke-width:4px,color:#000 + style CmdVel fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px + style BaseCtrl fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px + end + + %% ========== CONNECTIONS ========== + %% API to User Controller + CAPI -->|"🔗 P/Invoke"| UserCtrl + + %% User Controller to Interface + UserCtrl -->|"🎯 Uses"| BaseNav + + %% Interface to MoveBase + BaseNav -->|"⚙️ Implements"| MoveBase + + %% MoveBase to Planning + MoveBase -->|"🎛️ Manages"| GP + MoveBase -->|"🎛️ Manages"| LP + MoveBase -->|"🎛️ Manages"| RB + + %% MoveBase to Costmap + MoveBase -->|"🎛️ Manages"| GC + MoveBase -->|"🎛️ Manages"| LC + + %% Data Sources to Components + Goal -->|"📥 Input"| UserCtrl + Goal -->|"📥 Input"| BaseNav + Loc -->|"📍 Pose"| MoveBase + TF -->|"🔄 Transforms"| MoveBase + Laser -->|"📡 Scan Data"| LC + Map -->|"🗺️ Static Map"| GC + Odom -->|"⚡ Velocity"| LP + Odom -->|"📍 Pose"| MoveBase + + %% Planning Flow + GC -->|"🗺️ Costmap"| GP + GP -->|"🛤️ Global Path"| LP + LC -->|"🗺️ Costmap"| LP + LC -->|"🗺️ Costmap"| RB + GC -->|"🗺️ Costmap"| RB + + %% Algorithm Integration + MKTAlgo -->|"🚗 Kinematics"| LP + ScoreAlgo -->|"📊 Scoring"| LP + KalmanAlgo -.->|"🔍 Filtering"| Loc + + %% Control Flow + LP -->|"⚡ Velocity Cmd"| CmdVel + CmdVel -->|"▶️ Execute"| BaseCtrl + BaseCtrl -->|"📊 Feedback"| Odom + + %% Styling Classes + classDef implemented fill:#C8E6C9,stroke:#2E7D32,stroke-width:4px,color:#000 + classDef partial fill:#FFF9C4,stroke:#F57F17,stroke-width:3px,color:#000 + classDef todo fill:#FFCDD2,stroke:#C62828,stroke-width:3px,color:#000 + + class BaseNav,MoveBase,GP,LP,RB,GC,LC,CAPI,MKTAlgo,ScoreAlgo,KalmanAlgo,UserCtrl implemented + class BaseCtrl,Loc,Odom,Map,Laser,TF partial +``` + +## Đề xuất giải pháp kiến trúc + +### 1. Xác định phạm vi & yêu cầu vận hành + +Cần làm rõ: +- **Kịch bản sử dụng chính**: go-to-goal trong môi trường indoor/outdoor? Tốc độ di chuyển? Độ chính xác yêu cầu? +- **Cảm biến có sẵn**: encoder, IMU, lidar, camera? Dữ liệu nào đã có? +- **Yêu cầu real-time**: chu kỳ điều khiển (control loop frequency), độ trễ tối đa cho phép? + +### 2. Kiến trúc tổng thể + +**Các layer chính:** + +1. **API Layer** ✅ + - C API (`nav_c_api`) cho P/Invoke với .NET/C# + - Wrapper functions cho tất cả BaseNavigation methods + +2. **User Controller Layer** ✅ + - **User Controller Plugin**: Dynamically allocated từ loader controller plugin + - Sử dụng `boost::dll` để load plugins động + - Cho phép user định nghĩa controller riêng để điều khiển navigation behavior + - Kết nối với `BaseNavigation` interface + +3. **Interface Layer** ✅ + - `move_base_core::BaseNavigation`: Abstract interface cho navigation + - Định nghĩa các operations: moveTo, dockTo, rotateTo, moveStraightTo, pause, resume, cancel + - Là lớp trung gian định nghĩa các phương thức và thuộc tính liên quan đến navigation + +4. **Move Base Core** ✅ + - `move_base::MoveBase`: Core implementation của BaseNavigation + - Quản lý state machine (PLANNING, CONTROLLING, CLEARING) + - Điều phối global/local planner và recovery behaviors + - Quản lý costmaps (global và local) + - Thực thi control loop (executeCycle) + +4. **Planning Layer** ✅ + - `nav_core::BaseGlobalPlanner`: Interface cho global planners (A*, D*, etc.) + - `nav_core::BaseLocalPlanner`: Interface cho local planners (DWA, TEB, MKT, etc.) + - `nav_core::RecoveryBehavior`: Interface cho recovery behaviors + - Plugin system sử dụng `boost::dll` để dynamic loading + +5. **Costmap Layer** ✅ + - `costmap_2d::Costmap2DROBOT`: Global và local costmap + - Costmap layers: static map, obstacles, inflation + - Frame management: map (global), odom (local) + +6. **Algorithms Layer** ✅ + - `mkt_algorithm`: Diff drive và bicycle kinematics algorithms + - `score_algorithm`: Trajectory scoring và goal checking + - `kalman`: Filtering algorithms + +7. **Data Sources** ⚠️ (Interface cần định nghĩa) + - Localization source (Pnkx Loc) + - Odometry source + - Sensor transforms (tf3) + - Map server + - Laser sensors + +8. **Control Layer** ⚠️ (Cần implementation) + - Base Controller interface (Diff/Steer drive) + - Velocity command execution + +**Định dạng dữ liệu:** +- `geometry_msgs::Pose2D` / `geometry_msgs::PoseStamped` (vị trí + hướng) +- `geometry_msgs::Twist` (vận tốc linear/angular) +- `std::vector` (đường đi) +- `costmap_2d::Costmap2D` (bản đồ chi phí) + +### 3. Thiết kế từng module (interface level) + +**Các interface đã có:** ✅ + +- `move_base_core::BaseNavigation` ✅ + - `initialize(TFListenerPtr)` - Khởi tạo với TF listener + - `moveTo(goal, xy_tol, yaw_tol)` - Di chuyển đến goal + - `dockTo(marker, goal, ...)` - Docking đến marker + - `rotateTo(goal, yaw_tol)` - Xoay tại chỗ + - `moveStraightTo(goal, xy_tol)` - Đi thẳng + - `pause()`, `resume()`, `cancel()` - Điều khiển trạng thái + - `getRobotPose(...)` - Lấy vị trí robot + +- `nav_core::BaseGlobalPlanner` ✅ + - `makePlan(start, goal, plan)` - Tạo global path + - `initialize(name, costmap_robot)` - Khởi tạo với costmap + +- `nav_core::BaseLocalPlanner` ✅ + - `computeVelocityCommands(cmd_vel)` - Tính toán velocity command + - `setPlan(plan)` - Set global path để follow + - `isGoalReached()` - Kiểm tra đã đến goal chưa + - `swapPlanner(name)` - Thay đổi planner động + - `setTwistLinear/Angular(...)` - Set velocity limits + +- `nav_core::RecoveryBehavior` ✅ + - `runBehavior()` - Thực thi recovery behavior + +- `costmap_2d::Costmap2DROBOT` ✅ + - Wrapper cho costmap với robot footprint + - Thread-safe access với mutex + +**Các interface cần bổ sung:** ⚠️ + +- `ILocalizationSource` ⚠️ + - `getCurrentPose()` - Lấy vị trí hiện tại từ localization + - `isAvailable()` - Kiểm tra localization có sẵn không + +- `IOdometrySource` ⚠️ + - `getCurrentPose()` - Lấy vị trí từ odometry + - `getCurrentVelocity()` - Lấy vận tốc hiện tại + - `isAvailable()` - Kiểm tra odometry có sẵn không + +- `IBaseController` ⚠️ + - `executeVelocity(cmd_vel)` - Thực thi velocity command + - `stop()` - Dừng robot ngay lập tức + - `getCurrentVelocity()` - Lấy vận tốc thực tế + +- `IMapProvider` ⚠️ + - `getMap()` - Lấy static map + - `isMapAvailable()` - Kiểm tra map có sẵn không + +**Plugin mechanism:** ✅ +- Sử dụng `boost::dll` để dynamic loading plugins +- Factory pattern với `boost::function` và `boost::dll::import` +- Config file YAML để specify plugin names +- Plugin interfaces: `BaseGlobalPlanner`, `BaseLocalPlanner`, `RecoveryBehavior` + +### 4. Cơ chế giao tiếp & đồng bộ + +**Lựa chọn transport:** +- Shared memory + mutex (cho real-time nhẹ) +- Message queue (ZeroMQ, nanomsg) +- Event loop với callback thread-safe + +**Time synchronization:** +- Sử dụng `std::chrono` cho timestamp +- Buffer dữ liệu để xử lý dữ liệu không đồng bộ + +**Threading strategy:** +- Mỗi module một thread riêng, hoặc +- Scheduler chung quản lý tất cả + +### 5. Chiến lược an toàn & recovery + +**Monitoring:** +- Heartbeat mechanism +- Watchdog timer + +**Recovery behaviors:** +- Không tìm được đường đi +- Mất localization +- Obstacle chặn đường +- Emergency stop mechanism +- Giới hạn vận tốc theo trạng thái + +### 6. Config & Logging/Diagnostics + +**Configuration:** +- File YAML/JSON cho: + - Kinematics parameters + - Velocity limits + - Planner parameters + - Sensor configurations + +**Logging & Debugging:** +- Logging framework (spdlog?) +- Telemetry interface +- Visualizer tool (SDL/ImGui) để debug map/path + +### 7. Trạng thái triển khai & lộ trình + +**Đã hoàn thành:** ✅ + +1. ✅ **Interface Layer**: `BaseNavigation` interface +2. ✅ **Implementation Layer**: `MoveBase` core logic +3. ✅ **Planning Layer**: Plugin system cho global/local planners và recovery +4. ✅ **Costmap Layer**: Global và local costmap với layers +5. ✅ **Algorithms Layer**: MKT algorithms, score algorithm, kalman +6. ✅ **API Layer**: C API wrapper cho .NET integration +7. ✅ **Supporting Libraries**: tf3, robot_time, geometry_msgs, nav_2d_utils + +**Đang triển khai / Cần bổ sung:** ⚠️ + +1. ⚠️ **Data Sources Interfaces**: + - `ILocalizationSource` interface + - `IOdometrySource` interface + - `IMapProvider` interface + - Integration với Pnkx Loc, odometry sources + +2. ⚠️ **Base Controller**: + - `IBaseController` interface + - Diff drive controller implementation + - Steer drive controller implementation + - Velocity command execution + +3. ⚠️ **Control Loop**: + - Control loop trong MoveBase (executeCycle) + - State machine management hoàn chỉnh + - Threading và synchronization + +4. ⚠️ **User Controller Plugin System**: + - Factory để load user controller plugins + - Integration với BaseNavigation + +**Lộ trình tiếp theo:** + +**Phase 1: Data Sources & Base Controller** (Ưu tiên cao) +- Định nghĩa interfaces cho data sources +- Implement Base Controller interface và diff drive controller +- Integration với MoveBase + +**Phase 2: Control Loop & State Management** (Ưu tiên cao) +- Hoàn thiện executeCycle trong MoveBase +- State machine management +- Threading strategy + +**Phase 3: User Controller Plugin System** (Ưu tiên trung bình) +- Factory pattern cho user controllers +- Plugin loading mechanism +- Integration testing + +**Phase 4: Testing & Optimization** (Ưu tiên trung bình) +- Unit tests cho các module +- Integration tests +- Performance optimization + +**Testing strategy:** +- Unit tests cho các module độc lập (gtest?) +- Integration tests cho full navigation stack +- Simulation environment (2D simulator) - TODO +- Hardware-in-the-loop testing - TODO + +## Cấu trúc thư mục + +``` +pnkx_nav_core/ +├── src/ +│ ├── Navigations/ +│ │ ├── Cores/ +│ │ │ ├── move_base_core/ # BaseNavigation interface +│ │ │ ├── nav_core/ # Planner interfaces +│ │ │ ├── nav_core_adapter/ # Adapter utilities +│ │ │ └── nav_core2/ # Additional nav utilities +│ │ ├── Libraries/ +│ │ │ ├── costmap_2d/ # Costmap system +│ │ │ ├── tf3/ # Transform system +│ │ │ ├── robot_time/ # Time management +│ │ │ ├── geometry2/ # Geometry utilities +│ │ │ └── ... # Other supporting libraries +│ │ └── Packages/ +│ │ └── move_base/ # MoveBase implementation +│ ├── Algorithms/ +│ │ ├── Cores/ +│ │ │ └── score_algorithm/ # Trajectory scoring +│ │ └── Libraries/ +│ │ ├── mkt_algorithm/ # MKT kinematics algorithms +│ │ ├── kalman/ # Kalman filtering +│ │ └── angles/ # Angle utilities +│ └── APIs/ +│ └── c_api/ # C API wrapper +├── build/ # Build artifacts +└── doc/ # Documentation +``` + +## Ghi chú + +- ✅ Kiến trúc cốt lõi đã được triển khai với plugin system linh hoạt +- ⚠️ Cần bổ sung data sources interfaces và base controller +- 🔄 Kiến trúc được thiết kế để dễ dàng thay đổi thuật toán và mô hình kinematics thông qua plugin system +- 📦 Tất cả components được build bằng CMake, không phụ thuộc ROS +- 🔌 Plugin system sử dụng `boost::dll` cho dynamic loading + + + + + + diff --git a/doc/diff_drive_c.drawio b/doc/diff_drive_c.drawio new file mode 100755 index 0000000..72204be --- /dev/null +++ b/doc/diff_drive_c.drawio @@ -0,0 +1,1282 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/folders.md b/doc/folders.md new file mode 100644 index 0000000..c46d5ea --- /dev/null +++ b/doc/folders.md @@ -0,0 +1,53 @@ +Mô tả cấu trúc: + +├── common_msgs/ +│ ├── build/ +│ ├── geometry_msgs/ +│ │ ├── include/ +│ │ └── test/ +│ ├── CMakeLists.txt +│ ├── nav_msgs/ +│ │ ├── include/ +│ │ └── test/ +│ ├── CMakeLists.txt +│ ├── sensor_msgs/ +│ │ ├── cfg/ +│ │ ├── include/ +│ │ └── test/ +│ ├── CMakeLists.txt +│ ├── std_msgs/ +│ │ ├── include/ +│ │ └── CMakeLists.txt +│ └── CMakeLists.txt (root) +| +├── Navigations/ +│ ├── Cores/ +│ │ └── move_base_core/ +│ │ ├── build/ +│ │ ├── example/ +│ │ ├── include/ +│ │ ├── .gitignore +│ │ ├── CMakeLists.txt +│ │ └── README.md +│ ├── Libraries/ +│ │ ├── geometry_msgs/ +│ │ │ ├── build/ +│ │ │ ├── include/ +│ │ │ └── CMakeLists.txt +│ │ └── libtf2/ +│ │ ├── .vscode/ +│ │ ├── include/ +│ │ ├── samples/ +│ │ ├── src/ +│ │ ├── .gitignore +│ │ ├── DEBIAN_PACKAGING.md +│ │ ├── libtf2_2.0.0_amd64.deb +│ │ ├── Makefile +│ │ └── README.md +│ ├── Packages/ +│ │ └── move_base/ +│ │ ├── build/ +│ │ ├── include/ +│ │ ├── src/ +│ │ └── CMakeLists.txt +│ └── CMakeLists.txt (root) \ No newline at end of file diff --git a/doc/implementation_plan.md b/doc/implementation_plan.md new file mode 100644 index 0000000..e4c7ea7 --- /dev/null +++ b/doc/implementation_plan.md @@ -0,0 +1,230 @@ +# Kế hoạch Implementation Navigation System + +## Phân tích Kiến trúc Hiện tại + +### ✅ Đã có sẵn: + +1. **Interface Layer:** + - `move_base_core::BaseNavigation` - Interface chính + - `move_base::MoveBase` - Implementation + +2. **Core Components:** + - `nav_core::BaseGlobalPlanner` - Interface cho global planner + - `nav_core::BaseLocalPlanner` - Interface cho local planner + - `nav_core::RecoveryBehavior` - Interface cho recovery behaviors + - Plugin system với `boost::dll` để load dynamic plugins + +3. **Costmap System:** + - `costmap_2d::Costmap2DROBOT` - Global và local costmap + - Costmap layers (static, obstacle, inflation, etc.) + +4. **Supporting Libraries:** + - `tf3` - Transform system + - `robot_time` - Time management + - `geometry_msgs` - Message types + - `nav_2d_msgs`, `nav_2d_utils` - 2D navigation utilities + +### ❌ Cần bổ sung/hoàn thiện: + +1. **User Controller Plugin System:** + - Factory để tạo User Controller từ plugin + - Interface cho User Controller + - Integration với BaseNavigation + +2. **Data Sources Integration:** + - Pnkx Loc interface + - Odometry Source interface + - Sensor Transform management + - Map Server interface + - Laser Sensor Sources interface + +3. **Base Controller:** + - Diff drive controller + - Steer drive controller + - Velocity command execution + +4. **Hoàn thiện MoveBase:** + - Control loop implementation + - State machine management + - Recovery behavior execution + +## Kế hoạch Implementation + +### Phase 1: Data Sources & Interfaces (Ưu tiên cao) + +#### 1.1 Odometry Source Interface +**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/odometry_source.h` +```cpp +class IOdometrySource { + virtual geometry_msgs::PoseStamped getCurrentPose() = 0; + virtual geometry_msgs::Twist getCurrentVelocity() = 0; + virtual bool isAvailable() = 0; +}; +``` + +#### 1.2 Localization Source Interface (Pnkx Loc) +**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/localization_source.h` +```cpp +class ILocalizationSource { + virtual geometry_msgs::PoseStamped getRobotPose() = 0; + virtual std::string getGlobalFrame() = 0; + virtual bool isLocalized() = 0; +}; +``` + +#### 1.3 Map Provider Interface +**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/map_provider.h` +```cpp +class IMapProvider { + virtual nav_msgs::OccupancyGrid getMap() = 0; + virtual bool isMapAvailable() = 0; +}; +``` + +#### 1.4 Sensor Data Interface +**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/sensor_source.h` +```cpp +class ISensorSource { + virtual sensor_msgs::LaserScan getLatestScan() = 0; + virtual bool hasNewData() = 0; +}; +``` + +### Phase 2: Base Controller (Ưu tiên cao) + +#### 2.1 Base Controller Interface +**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/base_controller.h` +```cpp +class IBaseController { + virtual bool setVelocity(const geometry_msgs::Twist& cmd_vel) = 0; + virtual bool stop() = 0; + virtual geometry_msgs::Twist getCurrentVelocity() = 0; +}; +``` + +#### 2.2 Diff Drive Controller +**File:** `src/Navigations/Packages/base_controllers/include/base_controllers/diff_drive_controller.h` +- Implement IBaseController +- Convert Twist to wheel velocities +- Handle kinematics cho differential drive + +#### 2.3 Steer Drive Controller +**File:** `src/Navigations/Packages/base_controllers/include/base_controllers/steer_drive_controller.h` +- Implement IBaseController +- Convert Twist to steering angles và wheel velocities +- Handle kinematics cho bicycle/steering model + +### Phase 3: User Controller Plugin System (Ưu tiên trung bình) + +#### 3.1 User Controller Interface +**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/user_controller.h` +```cpp +class IUserController { + virtual void onGoalReceived(const geometry_msgs::PoseStamped& goal) = 0; + virtual void onNavigationStateChanged(move_base_core::State state) = 0; + virtual void onFeedback(const NavFeedback& feedback) = 0; +}; +``` + +#### 3.2 User Controller Factory +**File:** `src/Navigations/Cores/move_base_core/src/user_controller_factory.cpp` +- Load plugin từ config +- Create instance với boost::dll +- Manage lifecycle + +### Phase 4: Control Loop & State Machine (Ưu tiên cao) + +#### 4.1 Control Loop Implementation +**File:** `src/Navigations/Packages/move_base/src/move_base_control_loop.cpp` +- Main control loop thread +- Planner thread management +- Controller execution +- Frequency control + +#### 4.2 State Machine +**File:** `src/Navigations/Packages/move_base/src/move_base_state_machine.cpp` +- State transitions +- Recovery behavior triggers +- Goal management + +### Phase 5: Integration & Testing (Ưu tiên trung bình) + +#### 5.1 Integration Layer +- Connect tất cả components +- Data flow management +- Error handling + +#### 5.2 Testing +- Unit tests +- Integration tests +- Simulation tests + +## Cấu trúc Thư mục Đề xuất + +``` +src/Navigations/ +├── Cores/ +│ ├── move_base_core/ ✅ Đã có +│ │ ├── interfaces/ +│ │ │ ├── odometry_source.h ⚠️ Cần tạo +│ │ │ ├── localization_source.h ⚠️ Cần tạo +│ │ │ ├── map_provider.h ⚠️ Cần tạo +│ │ │ ├── sensor_source.h ⚠️ Cần tạo +│ │ │ ├── base_controller.h ⚠️ Cần tạo +│ │ │ └── user_controller.h ⚠️ Cần tạo +│ │ └── src/ +│ │ └── user_controller_factory.cpp ⚠️ Cần tạo +│ ├── nav_core/ ✅ Đã có +│ └── nav_core2/ ✅ Đã có +│ +├── Packages/ +│ ├── move_base/ ✅ Đã có (cần hoàn thiện) +│ │ └── src/ +│ │ ├── move_base_control_loop.cpp ⚠️ Cần tạo +│ │ └── move_base_state_machine.cpp ⚠️ Cần tạo +│ │ +│ └── base_controllers/ ⚠️ Cần tạo +│ ├── include/ +│ │ └── base_controllers/ +│ │ ├── diff_drive_controller.h +│ │ └── steer_drive_controller.h +│ └── src/ +│ ├── diff_drive_controller.cpp +│ └── steer_drive_controller.cpp +│ +└── Libraries/ + └── (các libraries hiện có) ✅ +``` + +## Thứ tự Implementation + +### Sprint 1: Foundation (1-2 tuần) +1. ✅ Tạo interfaces cho data sources +2. ✅ Tạo base controller interface +3. ✅ Implement diff drive controller cơ bản + +### Sprint 2: Integration (1-2 tuần) +1. ✅ Integrate data sources vào MoveBase +2. ✅ Implement control loop +3. ✅ Connect base controller + +### Sprint 3: Plugin System (1 tuần) +1. ✅ User controller plugin system +2. ✅ Factory pattern implementation + +### Sprint 4: State Machine & Recovery (1 tuần) +1. ✅ State machine implementation +2. ✅ Recovery behavior integration + +### Sprint 5: Testing & Polish (1 tuần) +1. ✅ Unit tests +2. ✅ Integration tests +3. ✅ Documentation + +## Notes + +- Tất cả interfaces nên follow pattern của `BaseNavigation` +- Sử dụng `boost::dll` cho plugin system (đã có sẵn) +- Thread safety: sử dụng `boost::mutex` và `boost::recursive_mutex` +- Error handling: throw exceptions, return bool cho simple operations + diff --git a/doc/readme.md b/doc/readme.md new file mode 100644 index 0000000..09ed57e --- /dev/null +++ b/doc/readme.md @@ -0,0 +1,15 @@ +Mô tả dựng án : + Tôi đang nghiên cứu viết chương di chuyển bám quỹ đạo (navigation) cho robot amr dạng hai bánh vi sai. Tôi muốn bỏ ROS chuyển sang viết + thuần trên Cmake. + +Các chức năng cần có: +- Có khả năng thay đổi mô hình kinematics như dạng hai bánh vi sai, dạng steering bicycle, ... +- Có chức năng di chuyển từ điểủ khởi đầu đến điểm đích +- Có chức năng xoay tròn +- Có chức năng đi thẳng +- Có chức năng thay đổi vận tốc + + + + + diff --git a/examples/CSharpExample.cs b/examples/CSharpExample.cs new file mode 100644 index 0000000..f3975af --- /dev/null +++ b/examples/CSharpExample.cs @@ -0,0 +1,322 @@ +using System; +using System.Runtime.InteropServices; +using System.Runtime.CompilerServices; + +namespace NavigationExample +{ + /// + /// C# P/Invoke wrapper for Navigation C API + /// + public class NavigationAPI + { + private const string DllName = "libnav_c_api.so"; // Linux + // For Windows: "nav_c_api.dll" + // For macOS: "libnav_c_api.dylib" + + // ============================================================================ + // Enums + // ============================================================================ + public enum NavigationState + { + Pending = 0, + Active = 1, + Preempted = 2, + Succeeded = 3, + Aborted = 4, + Rejected = 5, + Preempting = 6, + Recalling = 7, + Recalled = 8, + Lost = 9, + Planning = 10, + Controlling = 11, + Clearing = 12, + Paused = 13 + } + + // ============================================================================ + // Structures + // ============================================================================ + [StructLayout(LayoutKind.Sequential)] + public struct Point + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose2D + { + public double x; + public double y; + public double theta; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + public double x; + public double y; + public double z; + public double w; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Position + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose + { + public Position position; + public Quaternion orientation; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Header + { + public uint seq; + public long sec; + public uint nsec; + public IntPtr frame_id; // char* + } + + [StructLayout(LayoutKind.Sequential)] + public struct PoseStamped + { + public Header header; + public Pose pose; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct NavFeedback + { + public NavigationState navigation_state; + public IntPtr feed_back_str; // char* + public Pose2D current_pose; + [MarshalAs(UnmanagedType.I1)] + public bool goal_checked; + [MarshalAs(UnmanagedType.I1)] + public bool is_ready; + } + + // ============================================================================ + // String Management + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void nav_c_api_free_string(IntPtr str); + + // ============================================================================ + // State Conversion + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern IntPtr navigation_state_to_string(NavigationState state); + + // ============================================================================ + // Helper Functions + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_offset_goal_2d( + double pose_x, double pose_y, double pose_theta, + string frame_id, double offset_distance, + ref PoseStamped out_goal); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_offset_goal_stamped( + ref PoseStamped in_pose, double offset_distance, + ref PoseStamped out_goal); + + // ============================================================================ + // Navigation Handle Management + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr navigation_create(); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_destroy(IntPtr handle); + + // ============================================================================ + // TF Listener Management + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr tf_listener_create(); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void tf_listener_destroy(IntPtr handle); + + // ============================================================================ + // Navigation Interface Methods + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_robot_footprint( + IntPtr handle, Point[] points, UIntPtr point_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_to( + IntPtr handle, ref PoseStamped goal, + double xy_goal_tolerance, double yaw_goal_tolerance); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_dock_to( + IntPtr handle, string marker, ref PoseStamped goal, + double xy_goal_tolerance, double yaw_goal_tolerance); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_straight_to( + IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_rotate_to( + IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_pause(IntPtr handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_resume(IntPtr handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_cancel(IntPtr handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_twist_linear( + IntPtr handle, double linear_x, double linear_y, double linear_z); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_twist_angular( + IntPtr handle, double angular_x, double angular_y, double angular_z); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_stamped( + IntPtr handle, ref PoseStamped out_pose); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_2d( + IntPtr handle, ref Pose2D out_pose); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_feedback( + IntPtr handle, ref NavFeedback out_feedback); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_free_feedback(ref NavFeedback feedback); + + // ============================================================================ + // Helper Methods for String Conversion + // ============================================================================ + public static string MarshalString(IntPtr ptr) + { + if (ptr == IntPtr.Zero) + return string.Empty; + return Marshal.PtrToStringAnsi(ptr); + } + } + + // ============================================================================ + // Example Usage + // ============================================================================ + class Program + { + // Helper method để hiển thị file và line number tự động + static void LogError(string message, + [CallerFilePath] string filePath = "", + [CallerLineNumber] int lineNumber = 0, + [CallerMemberName] string memberName = "") + { + // Lấy tên file từ đường dẫn đầy đủ + string fileName = System.IO.Path.GetFileName(filePath); + Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}"); + } + + static void Main(string[] args) + { + // Create TF listener + IntPtr tfHandle = NavigationAPI.tf_listener_create(); + if (tfHandle == IntPtr.Zero) + { + LogError("Failed to create TF listener"); + return; + } + + // Create navigation instance + IntPtr navHandle = NavigationAPI.navigation_create(); + if (navHandle == IntPtr.Zero) + { + LogError("Failed to create navigation instance"); + NavigationAPI.tf_listener_destroy(tfHandle); + return; + } + + // Initialize navigation + if (!NavigationAPI.navigation_initialize(navHandle, tfHandle)) + { + LogError("Failed to initialize navigation"); + NavigationAPI.navigation_destroy(navHandle); + NavigationAPI.tf_listener_destroy(tfHandle); + return; + } + + // Set robot footprint + NavigationAPI.Point[] footprint = new NavigationAPI.Point[] + { + new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 }, + new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 }, + new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 }, + new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 } + }; + NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length)); + + // Get robot pose + NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D(); + if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose)) + { + Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}"); + } + + // Get navigation feedback + NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); + if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) + { + string stateStr = NavigationAPI.MarshalString( + NavigationAPI.navigation_state_to_string(feedback.navigation_state)); + string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str); + Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}"); + NavigationAPI.navigation_free_feedback(ref feedback); + } + + // Cleanup + NavigationAPI.navigation_destroy(navHandle); + NavigationAPI.tf_listener_destroy(tfHandle); + } + } +} + diff --git a/examples/NavigationExample/NavigationExample.csproj b/examples/NavigationExample/NavigationExample.csproj new file mode 100644 index 0000000..0d40596 --- /dev/null +++ b/examples/NavigationExample/NavigationExample.csproj @@ -0,0 +1,13 @@ + + + Exe + net6.0 + linux-x64 + true + + + + Always + + + diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs new file mode 100644 index 0000000..f3975af --- /dev/null +++ b/examples/NavigationExample/Program.cs @@ -0,0 +1,322 @@ +using System; +using System.Runtime.InteropServices; +using System.Runtime.CompilerServices; + +namespace NavigationExample +{ + /// + /// C# P/Invoke wrapper for Navigation C API + /// + public class NavigationAPI + { + private const string DllName = "libnav_c_api.so"; // Linux + // For Windows: "nav_c_api.dll" + // For macOS: "libnav_c_api.dylib" + + // ============================================================================ + // Enums + // ============================================================================ + public enum NavigationState + { + Pending = 0, + Active = 1, + Preempted = 2, + Succeeded = 3, + Aborted = 4, + Rejected = 5, + Preempting = 6, + Recalling = 7, + Recalled = 8, + Lost = 9, + Planning = 10, + Controlling = 11, + Clearing = 12, + Paused = 13 + } + + // ============================================================================ + // Structures + // ============================================================================ + [StructLayout(LayoutKind.Sequential)] + public struct Point + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose2D + { + public double x; + public double y; + public double theta; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + public double x; + public double y; + public double z; + public double w; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Position + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Pose + { + public Position position; + public Quaternion orientation; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Header + { + public uint seq; + public long sec; + public uint nsec; + public IntPtr frame_id; // char* + } + + [StructLayout(LayoutKind.Sequential)] + public struct PoseStamped + { + public Header header; + public Pose pose; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public double x; + public double y; + public double z; + } + + [StructLayout(LayoutKind.Sequential)] + public struct NavFeedback + { + public NavigationState navigation_state; + public IntPtr feed_back_str; // char* + public Pose2D current_pose; + [MarshalAs(UnmanagedType.I1)] + public bool goal_checked; + [MarshalAs(UnmanagedType.I1)] + public bool is_ready; + } + + // ============================================================================ + // String Management + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void nav_c_api_free_string(IntPtr str); + + // ============================================================================ + // State Conversion + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + public static extern IntPtr navigation_state_to_string(NavigationState state); + + // ============================================================================ + // Helper Functions + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_offset_goal_2d( + double pose_x, double pose_y, double pose_theta, + string frame_id, double offset_distance, + ref PoseStamped out_goal); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_offset_goal_stamped( + ref PoseStamped in_pose, double offset_distance, + ref PoseStamped out_goal); + + // ============================================================================ + // Navigation Handle Management + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr navigation_create(); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_destroy(IntPtr handle); + + // ============================================================================ + // TF Listener Management + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr tf_listener_create(); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void tf_listener_destroy(IntPtr handle); + + // ============================================================================ + // Navigation Interface Methods + // ============================================================================ + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_robot_footprint( + IntPtr handle, Point[] points, UIntPtr point_count); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_to( + IntPtr handle, ref PoseStamped goal, + double xy_goal_tolerance, double yaw_goal_tolerance); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_dock_to( + IntPtr handle, string marker, ref PoseStamped goal, + double xy_goal_tolerance, double yaw_goal_tolerance); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_straight_to( + IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_rotate_to( + IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_pause(IntPtr handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_resume(IntPtr handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_cancel(IntPtr handle); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_twist_linear( + IntPtr handle, double linear_x, double linear_y, double linear_z); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_set_twist_angular( + IntPtr handle, double angular_x, double angular_y, double angular_z); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_stamped( + IntPtr handle, ref PoseStamped out_pose); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_robot_pose_2d( + IntPtr handle, ref Pose2D out_pose); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_get_feedback( + IntPtr handle, ref NavFeedback out_feedback); + + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + public static extern void navigation_free_feedback(ref NavFeedback feedback); + + // ============================================================================ + // Helper Methods for String Conversion + // ============================================================================ + public static string MarshalString(IntPtr ptr) + { + if (ptr == IntPtr.Zero) + return string.Empty; + return Marshal.PtrToStringAnsi(ptr); + } + } + + // ============================================================================ + // Example Usage + // ============================================================================ + class Program + { + // Helper method để hiển thị file và line number tự động + static void LogError(string message, + [CallerFilePath] string filePath = "", + [CallerLineNumber] int lineNumber = 0, + [CallerMemberName] string memberName = "") + { + // Lấy tên file từ đường dẫn đầy đủ + string fileName = System.IO.Path.GetFileName(filePath); + Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}"); + } + + static void Main(string[] args) + { + // Create TF listener + IntPtr tfHandle = NavigationAPI.tf_listener_create(); + if (tfHandle == IntPtr.Zero) + { + LogError("Failed to create TF listener"); + return; + } + + // Create navigation instance + IntPtr navHandle = NavigationAPI.navigation_create(); + if (navHandle == IntPtr.Zero) + { + LogError("Failed to create navigation instance"); + NavigationAPI.tf_listener_destroy(tfHandle); + return; + } + + // Initialize navigation + if (!NavigationAPI.navigation_initialize(navHandle, tfHandle)) + { + LogError("Failed to initialize navigation"); + NavigationAPI.navigation_destroy(navHandle); + NavigationAPI.tf_listener_destroy(tfHandle); + return; + } + + // Set robot footprint + NavigationAPI.Point[] footprint = new NavigationAPI.Point[] + { + new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 }, + new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 }, + new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 }, + new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 } + }; + NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length)); + + // Get robot pose + NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D(); + if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose)) + { + Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}"); + } + + // Get navigation feedback + NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); + if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) + { + string stateStr = NavigationAPI.MarshalString( + NavigationAPI.navigation_state_to_string(feedback.navigation_state)); + string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str); + Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}"); + NavigationAPI.navigation_free_feedback(ref feedback); + } + + // Cleanup + NavigationAPI.navigation_destroy(navHandle); + NavigationAPI.tf_listener_destroy(tfHandle); + } + } +} + diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so new file mode 100755 index 0000000..4c1e0d1 Binary files /dev/null and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/examples/QUICK_START.md b/examples/QUICK_START.md new file mode 100644 index 0000000..b7f6eba --- /dev/null +++ b/examples/QUICK_START.md @@ -0,0 +1,148 @@ +# Quick Start - Chạy C# Example + +## Bước 1: Cài đặt .NET SDK (nếu chưa có) + +### Ubuntu/Debian: +```bash +# Cài đặt .NET 6.0 hoặc mới hơn +wget https://dot.net/v1/dotnet-install.sh +chmod +x dotnet-install.sh +./dotnet-install.sh --channel 6.0 + +# Thêm vào PATH +export PATH=$PATH:$HOME/.dotnet +export DOTNET_ROOT=$HOME/.dotnet + +# Hoặc cài qua apt (nếu có) +sudo apt-get update +sudo apt-get install -y dotnet-sdk-6.0 +``` + +### Kiểm tra: +```bash +dotnet --version +``` + +## Bước 2: Build C API Library + +```bash +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core +mkdir -p build +cd build +cmake .. +make nav_c_api +``` + +Library sẽ ở: `build/src/APIs/c_api/libnav_c_api.so` + +## Bước 3: Chạy C# Example + +### Cách 1: Dùng script tự động (Khuyến nghị) + +```bash +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples +chmod +x run_example.sh +./run_example.sh +``` + +Script sẽ tự động: +- ✅ Build library nếu chưa có +- ✅ Tạo C# project +- ✅ Copy library và source code +- ✅ Set LD_LIBRARY_PATH cho tất cả dependencies +- ✅ Build và chạy example + +### Cách 2: Chạy thủ công + +```bash +# 1. Tạo project +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples +dotnet new console -n NavigationExample +cd NavigationExample + +# 2. Copy source code +cp ../CSharpExample.cs Program.cs + +# 3. Tạo .csproj +cat > NavigationExample.csproj << 'EOF' + + + Exe + net6.0 + linux-x64 + true + + + + PreserveNewest + + + +EOF + +# 4. Copy library +cp ../../build/src/APIs/c_api/libnav_c_api.so . + +# 5. Set library path (quan trọng!) +export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH + +# 6. Build và chạy +dotnet build +dotnet run +``` + +## Bước 4: Kiểm tra kết quả + +Nếu chạy thành công, bạn sẽ thấy output như: +``` +Robot pose: x=0.0, y=0.0, theta=0.0 +State: PENDING, Feedback: ... +``` + +## Troubleshooting + +### Lỗi: "dotnet: command not found" +→ Cài đặt .NET SDK (xem Bước 1) + +### Lỗi: "Unable to load DLL 'libnav_c_api.so'" +```bash +# Kiểm tra library có tồn tại không +ls -la ../../build/src/APIs/c_api/libnav_c_api.so + +# Set LD_LIBRARY_PATH (bao gồm tất cả dependencies) +export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH + +# Hoặc copy vào system lib +sudo cp libnav_c_api.so /usr/local/lib/ +sudo ldconfig +``` + +### Lỗi: "Missing dependencies" +```bash +# Kiểm tra dependencies +ldd libnav_c_api.so + +# Đảm bảo tất cả dependencies đều có sẵn +``` + +### Lỗi: "Failed to create navigation instance" +→ Đã được sửa! `navigation_create()` bây giờ tạo instance của `move_base::MoveBase`. + Nếu vẫn gặp lỗi, kiểm tra: + - Library đã được build đầy đủ: `make nav_c_api` trong build directory + - Dependencies đã được build: `make move_base costmap_2d` + - LD_LIBRARY_PATH đã được set đúng + diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 0000000..d30fdf7 --- /dev/null +++ b/examples/README.md @@ -0,0 +1,142 @@ +# Hướng dẫn chạy C# Example + +## Yêu cầu + +1. **.NET SDK** (version 6.0 trở lên hoặc .NET 10) +2. **C API Library** đã được build (`libnav_c_api.so`) +3. **Linux environment** (hoặc WSL nếu dùng Windows) + +## Bước 1: Build C API Library + +```bash +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core +mkdir -p build +cd build +cmake .. +make nav_c_api +``` + +Library sẽ được tạo tại: `build/src/APIs/c_api/libnav_c_api.so` + +## Bước 2: Chạy script tự động (Khuyến nghị) + +```bash +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples +chmod +x run_example.sh +./run_example.sh +``` + +Script sẽ tự động: +- Build library nếu chưa có +- Tạo C# project +- Copy library và source code +- Set LD_LIBRARY_PATH cho tất cả dependencies +- Build và chạy example + +## Bước 3: Hoặc chạy thủ công + +### Tạo C# Project + +```bash +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples +dotnet new console -n NavigationExample +cd NavigationExample +``` + +### Copy file và library + +```bash +# Copy C# source code +cp ../CSharpExample.cs Program.cs + +# Copy library vào thư mục project +cp ../../build/src/APIs/c_api/libnav_c_api.so . +``` + +## Bước 4: Cập nhật .csproj (nếu cần) + +Tạo file `NavigationExample.csproj`: + +```xml + + + Exe + net6.0 + linux-x64 + true + + + + PreserveNewest + + + +``` + +## Bước 4: Build và chạy + +```bash +# Set LD_LIBRARY_PATH để tìm được tất cả dependencies +export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH +export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH + +# Build +dotnet build + +# Chạy +dotnet run +``` + +## Lưu ý + +1. **Library path**: Đảm bảo `libnav_c_api.so` có thể được tìm thấy: + - Copy vào cùng thư mục với executable + - Hoặc set `LD_LIBRARY_PATH` environment variable + - Hoặc copy vào `/usr/local/lib` và chạy `ldconfig` + +2. **Dependencies**: Library cần các dependencies khác: + ```bash + # Kiểm tra dependencies + ldd libnav_c_api.so + + # Đảm bảo tất cả dependencies đều có sẵn + ``` + +3. **Permissions**: Đảm bảo file có quyền execute: + ```bash + chmod +x libnav_c_api.so + ``` + +## Troubleshooting + +### Lỗi: "Unable to load DLL 'libnav_c_api.so'" + +**Giải pháp:** +```bash +# Set LD_LIBRARY_PATH +export LD_LIBRARY_PATH=/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/APIs/c_api:$LD_LIBRARY_PATH + +# Hoặc copy library vào system path +sudo cp libnav_c_api.so /usr/local/lib/ +sudo ldconfig +``` + +### Lỗi: "DllNotFoundException" + +**Giải pháp:** +- Kiểm tra tên file library có đúng không +- Kiểm tra architecture (x64 vs x86) +- Kiểm tra dependencies: `ldd libnav_c_api.so` + +### Lỗi: "Symbol not found" + +**Giải pháp:** +- Đảm bảo library đã được build đầy đủ +- Kiểm tra tên hàm trong C# có khớp với C API không + diff --git a/examples/START_HERE.md b/examples/START_HERE.md new file mode 100644 index 0000000..de518cb --- /dev/null +++ b/examples/START_HERE.md @@ -0,0 +1,83 @@ +# 🚀 Hướng dẫn chạy thử C API Navigation + +## Cách nhanh nhất (1 lệnh) + +```bash +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples +chmod +x run_example.sh +./run_example.sh +``` + +## Chi tiết các bước + +### Bước 1: Build C API Library + +```bash +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build +cmake .. +make nav_c_api +``` + +Library sẽ được tạo tại: `build/src/APIs/c_api/libnav_c_api.so` + +### Bước 2: Chạy C# Example + +Script `run_example.sh` sẽ tự động: +- ✅ Kiểm tra và build library nếu cần +- ✅ Tạo C# project +- ✅ Copy library và source code +- ✅ Set LD_LIBRARY_PATH cho tất cả dependencies +- ✅ Build và chạy example + +```bash +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/examples +./run_example.sh +``` + +## Kết quả mong đợi + +Nếu chạy thành công, bạn sẽ thấy: + +``` +========================================== +Building and Running C# Navigation Example +========================================== +Library already exists: .../build/src/APIs/c_api/libnav_c_api.so +C# project already exists +Copying library... +Building C# project... + +Running example... +========================================== +Robot pose: x=0, y=0, theta=0 +State: PENDING, Feedback: +========================================== +Done! +``` + +## Yêu cầu + +1. **.NET SDK 6.0+** (script sẽ kiểm tra và hướng dẫn nếu thiếu) +2. **Library đã được build** (script sẽ tự động build nếu thiếu) +3. **Linux environment** + +## Troubleshooting + +### Lỗi: "dotnet: command not found" +→ Script sẽ hiển thị hướng dẫn cài đặt .NET SDK + +### Lỗi: "Failed to create navigation instance" +→ Đảm bảo `move_base` đã được build: +```bash +cd build && make move_base +``` + +### Lỗi: "Cannot load library" +→ Kiểm tra LD_LIBRARY_PATH đã được set đúng (script tự động set) + +## Xem thêm + +- `README.md` - Hướng dẫn chi tiết +- `QUICK_START.md` - Quick start guide +- `run_example.sh` - Script tự động + diff --git a/examples/dotnet-install.sh b/examples/dotnet-install.sh new file mode 100755 index 0000000..0e19528 --- /dev/null +++ b/examples/dotnet-install.sh @@ -0,0 +1,1888 @@ +#!/usr/bin/env bash +# Copyright (c) .NET Foundation and contributors. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for full license information. +# + +# Stop script on NZEC +set -e +# Stop script if unbound variable found (use ${var:-} if intentional) +set -u +# By default cmd1 | cmd2 returns exit code of cmd2 regardless of cmd1 success +# This is causing it to fail +set -o pipefail + +# Use in the the functions: eval $invocation +invocation='say_verbose "Calling: ${yellow:-}${FUNCNAME[0]} ${green:-}$*${normal:-}"' + +# standard output may be used as a return value in the functions +# we need a way to write text on the screen in the functions so that +# it won't interfere with the return value. +# Exposing stream 3 as a pipe to standard output of the script itself +exec 3>&1 + +# Setup some colors to use. These need to work in fairly limited shells, like the Ubuntu Docker container where there are only 8 colors. +# See if stdout is a terminal +if [ -t 1 ] && command -v tput > /dev/null; then + # see if it supports colors + ncolors=$(tput colors || echo 0) + if [ -n "$ncolors" ] && [ $ncolors -ge 8 ]; then + bold="$(tput bold || echo)" + normal="$(tput sgr0 || echo)" + black="$(tput setaf 0 || echo)" + red="$(tput setaf 1 || echo)" + green="$(tput setaf 2 || echo)" + yellow="$(tput setaf 3 || echo)" + blue="$(tput setaf 4 || echo)" + magenta="$(tput setaf 5 || echo)" + cyan="$(tput setaf 6 || echo)" + white="$(tput setaf 7 || echo)" + fi +fi + +say_warning() { + printf "%b\n" "${yellow:-}dotnet_install: Warning: $1${normal:-}" >&3 +} + +say_err() { + printf "%b\n" "${red:-}dotnet_install: Error: $1${normal:-}" >&2 +} + +say() { + # using stream 3 (defined in the beginning) to not interfere with stdout of functions + # which may be used as return value + printf "%b\n" "${cyan:-}dotnet-install:${normal:-} $1" >&3 +} + +say_verbose() { + if [ "$verbose" = true ]; then + say "$1" + fi +} + +# This platform list is finite - if the SDK/Runtime has supported Linux distribution-specific assets, +# then and only then should the Linux distribution appear in this list. +# Adding a Linux distribution to this list does not imply distribution-specific support. +get_legacy_os_name_from_platform() { + eval $invocation + + platform="$1" + case "$platform" in + "centos.7") + echo "centos" + return 0 + ;; + "debian.8") + echo "debian" + return 0 + ;; + "debian.9") + echo "debian.9" + return 0 + ;; + "fedora.23") + echo "fedora.23" + return 0 + ;; + "fedora.24") + echo "fedora.24" + return 0 + ;; + "fedora.27") + echo "fedora.27" + return 0 + ;; + "fedora.28") + echo "fedora.28" + return 0 + ;; + "opensuse.13.2") + echo "opensuse.13.2" + return 0 + ;; + "opensuse.42.1") + echo "opensuse.42.1" + return 0 + ;; + "opensuse.42.3") + echo "opensuse.42.3" + return 0 + ;; + "rhel.7"*) + echo "rhel" + return 0 + ;; + "ubuntu.14.04") + echo "ubuntu" + return 0 + ;; + "ubuntu.16.04") + echo "ubuntu.16.04" + return 0 + ;; + "ubuntu.16.10") + echo "ubuntu.16.10" + return 0 + ;; + "ubuntu.18.04") + echo "ubuntu.18.04" + return 0 + ;; + "alpine.3.4.3") + echo "alpine" + return 0 + ;; + esac + return 1 +} + +get_legacy_os_name() { + eval $invocation + + local uname=$(uname) + if [ "$uname" = "Darwin" ]; then + echo "osx" + return 0 + elif [ -n "$runtime_id" ]; then + echo $(get_legacy_os_name_from_platform "${runtime_id%-*}" || echo "${runtime_id%-*}") + return 0 + else + if [ -e /etc/os-release ]; then + . /etc/os-release + os=$(get_legacy_os_name_from_platform "$ID${VERSION_ID:+.${VERSION_ID}}" || echo "") + if [ -n "$os" ]; then + echo "$os" + return 0 + fi + fi + fi + + say_verbose "Distribution specific OS name and version could not be detected: UName = $uname" + return 1 +} + +get_linux_platform_name() { + eval $invocation + + if [ -n "$runtime_id" ]; then + echo "${runtime_id%-*}" + return 0 + else + if [ -e /etc/os-release ]; then + . /etc/os-release + echo "$ID${VERSION_ID:+.${VERSION_ID}}" + return 0 + elif [ -e /etc/redhat-release ]; then + local redhatRelease=$(&1 || true) | grep -q musl +} + +get_current_os_name() { + eval $invocation + + local uname=$(uname) + if [ "$uname" = "Darwin" ]; then + echo "osx" + return 0 + elif [ "$uname" = "FreeBSD" ]; then + echo "freebsd" + return 0 + elif [ "$uname" = "Linux" ]; then + local linux_platform_name="" + linux_platform_name="$(get_linux_platform_name)" || true + + if [ "$linux_platform_name" = "rhel.6" ]; then + echo $linux_platform_name + return 0 + elif is_musl_based_distro; then + echo "linux-musl" + return 0 + elif [ "$linux_platform_name" = "linux-musl" ]; then + echo "linux-musl" + return 0 + else + echo "linux" + return 0 + fi + fi + + say_err "OS name could not be detected: UName = $uname" + return 1 +} + +machine_has() { + eval $invocation + + command -v "$1" > /dev/null 2>&1 + return $? +} + +check_min_reqs() { + local hasMinimum=false + if machine_has "curl"; then + hasMinimum=true + elif machine_has "wget"; then + hasMinimum=true + fi + + if [ "$hasMinimum" = "false" ]; then + say_err "curl (recommended) or wget are required to download dotnet. Install missing prerequisite to proceed." + return 1 + fi + return 0 +} + +# args: +# input - $1 +to_lowercase() { + #eval $invocation + + echo "$1" | tr '[:upper:]' '[:lower:]' + return 0 +} + +# args: +# input - $1 +remove_trailing_slash() { + #eval $invocation + + local input="${1:-}" + echo "${input%/}" + return 0 +} + +# args: +# input - $1 +remove_beginning_slash() { + #eval $invocation + + local input="${1:-}" + echo "${input#/}" + return 0 +} + +# args: +# root_path - $1 +# child_path - $2 - this parameter can be empty +combine_paths() { + eval $invocation + + # TODO: Consider making it work with any number of paths. For now: + if [ ! -z "${3:-}" ]; then + say_err "combine_paths: Function takes two parameters." + return 1 + fi + + local root_path="$(remove_trailing_slash "$1")" + local child_path="$(remove_beginning_slash "${2:-}")" + say_verbose "combine_paths: root_path=$root_path" + say_verbose "combine_paths: child_path=$child_path" + echo "$root_path/$child_path" + return 0 +} + +get_machine_architecture() { + eval $invocation + + if command -v uname > /dev/null; then + CPUName=$(uname -m) + case $CPUName in + armv1*|armv2*|armv3*|armv4*|armv5*|armv6*) + echo "armv6-or-below" + return 0 + ;; + armv*l) + echo "arm" + return 0 + ;; + aarch64|arm64) + if [ "$(getconf LONG_BIT)" -lt 64 ]; then + # This is 32-bit OS running on 64-bit CPU (for example Raspberry Pi OS) + echo "arm" + return 0 + fi + echo "arm64" + return 0 + ;; + s390x) + echo "s390x" + return 0 + ;; + ppc64le) + echo "ppc64le" + return 0 + ;; + loongarch64) + echo "loongarch64" + return 0 + ;; + riscv64) + echo "riscv64" + return 0 + ;; + powerpc|ppc) + echo "ppc" + return 0 + ;; + esac + fi + + # Always default to 'x64' + echo "x64" + return 0 +} + +# args: +# architecture - $1 +get_normalized_architecture_from_architecture() { + eval $invocation + + local architecture="$(to_lowercase "$1")" + + if [[ $architecture == \ ]]; then + machine_architecture="$(get_machine_architecture)" + if [[ "$machine_architecture" == "armv6-or-below" ]]; then + say_err "Architecture \`$machine_architecture\` not supported. If you think this is a bug, report it at https://github.com/dotnet/install-scripts/issues" + return 1 + fi + + echo $machine_architecture + return 0 + fi + + case "$architecture" in + amd64|x64) + echo "x64" + return 0 + ;; + arm) + echo "arm" + return 0 + ;; + arm64) + echo "arm64" + return 0 + ;; + s390x) + echo "s390x" + return 0 + ;; + ppc64le) + echo "ppc64le" + return 0 + ;; + loongarch64) + echo "loongarch64" + return 0 + ;; + esac + + say_err "Architecture \`$architecture\` not supported. If you think this is a bug, report it at https://github.com/dotnet/install-scripts/issues" + return 1 +} + +# args: +# version - $1 +# channel - $2 +# architecture - $3 +get_normalized_architecture_for_specific_sdk_version() { + eval $invocation + + local is_version_support_arm64="$(is_arm64_supported "$1")" + local is_channel_support_arm64="$(is_arm64_supported "$2")" + local architecture="$3"; + local osname="$(get_current_os_name)" + + if [ "$osname" == "osx" ] && [ "$architecture" == "arm64" ] && { [ "$is_version_support_arm64" = false ] || [ "$is_channel_support_arm64" = false ]; }; then + #check if rosetta is installed + if [ "$(/usr/bin/pgrep oahd >/dev/null 2>&1;echo $?)" -eq 0 ]; then + say_verbose "Changing user architecture from '$architecture' to 'x64' because .NET SDKs prior to version 6.0 do not support arm64." + echo "x64" + return 0; + else + say_err "Architecture \`$architecture\` is not supported for .NET SDK version \`$version\`. Please install Rosetta to allow emulation of the \`$architecture\` .NET SDK on this platform" + return 1 + fi + fi + + echo "$architecture" + return 0 +} + +# args: +# version or channel - $1 +is_arm64_supported() { + # Extract the major version by splitting on the dot + major_version="${1%%.*}" + + # Check if the major version is a valid number and less than 6 + case "$major_version" in + [0-9]*) + if [ "$major_version" -lt 6 ]; then + echo false + return 0 + fi + ;; + esac + + echo true + return 0 +} + +# args: +# user_defined_os - $1 +get_normalized_os() { + eval $invocation + + local osname="$(to_lowercase "$1")" + if [ ! -z "$osname" ]; then + case "$osname" in + osx | freebsd | rhel.6 | linux-musl | linux) + echo "$osname" + return 0 + ;; + macos) + osname='osx' + echo "$osname" + return 0 + ;; + *) + say_err "'$user_defined_os' is not a supported value for --os option, supported values are: osx, macos, linux, linux-musl, freebsd, rhel.6. If you think this is a bug, report it at https://github.com/dotnet/install-scripts/issues." + return 1 + ;; + esac + else + osname="$(get_current_os_name)" || return 1 + fi + echo "$osname" + return 0 +} + +# args: +# quality - $1 +get_normalized_quality() { + eval $invocation + + local quality="$(to_lowercase "$1")" + if [ ! -z "$quality" ]; then + case "$quality" in + daily | preview) + echo "$quality" + return 0 + ;; + ga) + #ga quality is available without specifying quality, so normalizing it to empty + return 0 + ;; + *) + say_err "'$quality' is not a supported value for --quality option. Supported values are: daily, preview, ga. If you think this is a bug, report it at https://github.com/dotnet/install-scripts/issues." + return 1 + ;; + esac + fi + return 0 +} + +# args: +# channel - $1 +get_normalized_channel() { + eval $invocation + + local channel="$(to_lowercase "$1")" + + if [[ $channel == current ]]; then + say_warning 'Value "Current" is deprecated for -Channel option. Use "STS" instead.' + fi + + if [[ $channel == release/* ]]; then + say_warning 'Using branch name with -Channel option is no longer supported with newer releases. Use -Quality option with a channel in X.Y format instead.'; + fi + + if [ ! -z "$channel" ]; then + case "$channel" in + lts) + echo "LTS" + return 0 + ;; + sts) + echo "STS" + return 0 + ;; + current) + echo "STS" + return 0 + ;; + *) + echo "$channel" + return 0 + ;; + esac + fi + + return 0 +} + +# args: +# runtime - $1 +get_normalized_product() { + eval $invocation + + local product="" + local runtime="$(to_lowercase "$1")" + if [[ "$runtime" == "dotnet" ]]; then + product="dotnet-runtime" + elif [[ "$runtime" == "aspnetcore" ]]; then + product="aspnetcore-runtime" + elif [ -z "$runtime" ]; then + product="dotnet-sdk" + fi + echo "$product" + return 0 +} + +# The version text returned from the feeds is a 1-line or 2-line string: +# For the SDK and the dotnet runtime (2 lines): +# Line 1: # commit_hash +# Line 2: # 4-part version +# For the aspnetcore runtime (1 line): +# Line 1: # 4-part version + +# args: +# version_text - stdin +get_version_from_latestversion_file_content() { + eval $invocation + + cat | tail -n 1 | sed 's/\r$//' + return 0 +} + +# args: +# install_root - $1 +# relative_path_to_package - $2 +# specific_version - $3 +is_dotnet_package_installed() { + eval $invocation + + local install_root="$1" + local relative_path_to_package="$2" + local specific_version="${3//[$'\t\r\n']}" + + local dotnet_package_path="$(combine_paths "$(combine_paths "$install_root" "$relative_path_to_package")" "$specific_version")" + say_verbose "is_dotnet_package_installed: dotnet_package_path=$dotnet_package_path" + + if [ -d "$dotnet_package_path" ]; then + return 0 + else + return 1 + fi +} + +# args: +# downloaded file - $1 +# remote_file_size - $2 +validate_remote_local_file_sizes() +{ + eval $invocation + + local downloaded_file="$1" + local remote_file_size="$2" + local file_size='' + + if [[ "$OSTYPE" == "linux-gnu"* ]]; then + file_size="$(stat -c '%s' "$downloaded_file")" + elif [[ "$OSTYPE" == "darwin"* ]]; then + # hardcode in order to avoid conflicts with GNU stat + file_size="$(/usr/bin/stat -f '%z' "$downloaded_file")" + fi + + if [ -n "$file_size" ]; then + say "Downloaded file size is $file_size bytes." + + if [ -n "$remote_file_size" ] && [ -n "$file_size" ]; then + if [ "$remote_file_size" -ne "$file_size" ]; then + say "The remote and local file sizes are not equal. The remote file size is $remote_file_size bytes and the local size is $file_size bytes. The local package may be corrupted." + else + say "The remote and local file sizes are equal." + fi + fi + + else + say "Either downloaded or local package size can not be measured. One of them may be corrupted." + fi +} + +# args: +# azure_feed - $1 +# channel - $2 +# normalized_architecture - $3 +get_version_from_latestversion_file() { + eval $invocation + + local azure_feed="$1" + local channel="$2" + local normalized_architecture="$3" + + local version_file_url=null + if [[ "$runtime" == "dotnet" ]]; then + version_file_url="$azure_feed/Runtime/$channel/latest.version" + elif [[ "$runtime" == "aspnetcore" ]]; then + version_file_url="$azure_feed/aspnetcore/Runtime/$channel/latest.version" + elif [ -z "$runtime" ]; then + version_file_url="$azure_feed/Sdk/$channel/latest.version" + else + say_err "Invalid value for \$runtime" + return 1 + fi + say_verbose "get_version_from_latestversion_file: latest url: $version_file_url" + + download "$version_file_url" || return $? + return 0 +} + +# args: +# json_file - $1 +parse_globaljson_file_for_version() { + eval $invocation + + local json_file="$1" + if [ ! -f "$json_file" ]; then + say_err "Unable to find \`$json_file\`" + return 1 + fi + + sdk_section=$(cat $json_file | tr -d "\r" | awk '/"sdk"/,/}/') + if [ -z "$sdk_section" ]; then + say_err "Unable to parse the SDK node in \`$json_file\`" + return 1 + fi + + sdk_list=$(echo $sdk_section | awk -F"[{}]" '{print $2}') + sdk_list=${sdk_list//[\" ]/} + sdk_list=${sdk_list//,/$'\n'} + + local version_info="" + while read -r line; do + IFS=: + while read -r key value; do + if [[ "$key" == "version" ]]; then + version_info=$value + fi + done <<< "$line" + done <<< "$sdk_list" + if [ -z "$version_info" ]; then + say_err "Unable to find the SDK:version node in \`$json_file\`" + return 1 + fi + + unset IFS; + echo "$version_info" + return 0 +} + +# args: +# azure_feed - $1 +# channel - $2 +# normalized_architecture - $3 +# version - $4 +# json_file - $5 +get_specific_version_from_version() { + eval $invocation + + local azure_feed="$1" + local channel="$2" + local normalized_architecture="$3" + local version="$(to_lowercase "$4")" + local json_file="$5" + + if [ -z "$json_file" ]; then + if [[ "$version" == "latest" ]]; then + local version_info + version_info="$(get_version_from_latestversion_file "$azure_feed" "$channel" "$normalized_architecture" false)" || return 1 + say_verbose "get_specific_version_from_version: version_info=$version_info" + echo "$version_info" | get_version_from_latestversion_file_content + return 0 + else + echo "$version" + return 0 + fi + else + local version_info + version_info="$(parse_globaljson_file_for_version "$json_file")" || return 1 + echo "$version_info" + return 0 + fi +} + +# args: +# azure_feed - $1 +# channel - $2 +# normalized_architecture - $3 +# specific_version - $4 +# normalized_os - $5 +construct_download_link() { + eval $invocation + + local azure_feed="$1" + local channel="$2" + local normalized_architecture="$3" + local specific_version="${4//[$'\t\r\n']}" + local specific_product_version="$(get_specific_product_version "$1" "$4")" + local osname="$5" + + local download_link=null + if [[ "$runtime" == "dotnet" ]]; then + download_link="$azure_feed/Runtime/$specific_version/dotnet-runtime-$specific_product_version-$osname-$normalized_architecture.tar.gz" + elif [[ "$runtime" == "aspnetcore" ]]; then + download_link="$azure_feed/aspnetcore/Runtime/$specific_version/aspnetcore-runtime-$specific_product_version-$osname-$normalized_architecture.tar.gz" + elif [ -z "$runtime" ]; then + download_link="$azure_feed/Sdk/$specific_version/dotnet-sdk-$specific_product_version-$osname-$normalized_architecture.tar.gz" + else + return 1 + fi + + echo "$download_link" + return 0 +} + +# args: +# azure_feed - $1 +# specific_version - $2 +# download link - $3 (optional) +get_specific_product_version() { + # If we find a 'productVersion.txt' at the root of any folder, we'll use its contents + # to resolve the version of what's in the folder, superseding the specified version. + # if 'productVersion.txt' is missing but download link is already available, product version will be taken from download link + eval $invocation + + local azure_feed="$1" + local specific_version="${2//[$'\t\r\n']}" + local package_download_link="" + if [ $# -gt 2 ]; then + local package_download_link="$3" + fi + local specific_product_version=null + + # Try to get the version number, using the productVersion.txt file located next to the installer file. + local download_links=($(get_specific_product_version_url "$azure_feed" "$specific_version" true "$package_download_link") + $(get_specific_product_version_url "$azure_feed" "$specific_version" false "$package_download_link")) + + for download_link in "${download_links[@]}" + do + say_verbose "Checking for the existence of $download_link" + + if machine_has "curl" + then + if ! specific_product_version=$(curl -s --fail "${download_link}${feed_credential}" 2>&1); then + continue + else + echo "${specific_product_version//[$'\t\r\n']}" + return 0 + fi + + elif machine_has "wget" + then + specific_product_version=$(wget -qO- "${download_link}${feed_credential}" 2>&1) + if [ $? = 0 ]; then + echo "${specific_product_version//[$'\t\r\n']}" + return 0 + fi + fi + done + + # Getting the version number with productVersion.txt has failed. Try parsing the download link for a version number. + say_verbose "Failed to get the version using productVersion.txt file. Download link will be parsed instead." + specific_product_version="$(get_product_specific_version_from_download_link "$package_download_link" "$specific_version")" + echo "${specific_product_version//[$'\t\r\n']}" + return 0 +} + +# args: +# azure_feed - $1 +# specific_version - $2 +# is_flattened - $3 +# download link - $4 (optional) +get_specific_product_version_url() { + eval $invocation + + local azure_feed="$1" + local specific_version="$2" + local is_flattened="$3" + local package_download_link="" + if [ $# -gt 3 ]; then + local package_download_link="$4" + fi + + local pvFileName="productVersion.txt" + if [ "$is_flattened" = true ]; then + if [ -z "$runtime" ]; then + pvFileName="sdk-productVersion.txt" + elif [[ "$runtime" == "dotnet" ]]; then + pvFileName="runtime-productVersion.txt" + else + pvFileName="$runtime-productVersion.txt" + fi + fi + + local download_link=null + + if [ -z "$package_download_link" ]; then + if [[ "$runtime" == "dotnet" ]]; then + download_link="$azure_feed/Runtime/$specific_version/${pvFileName}" + elif [[ "$runtime" == "aspnetcore" ]]; then + download_link="$azure_feed/aspnetcore/Runtime/$specific_version/${pvFileName}" + elif [ -z "$runtime" ]; then + download_link="$azure_feed/Sdk/$specific_version/${pvFileName}" + else + return 1 + fi + else + download_link="${package_download_link%/*}/${pvFileName}" + fi + + say_verbose "Constructed productVersion link: $download_link" + echo "$download_link" + return 0 +} + +# args: +# download link - $1 +# specific version - $2 +get_product_specific_version_from_download_link() +{ + eval $invocation + + local download_link="$1" + local specific_version="$2" + local specific_product_version="" + + if [ -z "$download_link" ]; then + echo "$specific_version" + return 0 + fi + + #get filename + filename="${download_link##*/}" + + #product specific version follows the product name + #for filename 'dotnet-sdk-3.1.404-linux-x64.tar.gz': the product version is 3.1.404 + IFS='-' + read -ra filename_elems <<< "$filename" + count=${#filename_elems[@]} + if [[ "$count" -gt 2 ]]; then + specific_product_version="${filename_elems[2]}" + else + specific_product_version=$specific_version + fi + unset IFS; + echo "$specific_product_version" + return 0 +} + +# args: +# azure_feed - $1 +# channel - $2 +# normalized_architecture - $3 +# specific_version - $4 +construct_legacy_download_link() { + eval $invocation + + local azure_feed="$1" + local channel="$2" + local normalized_architecture="$3" + local specific_version="${4//[$'\t\r\n']}" + + local distro_specific_osname + distro_specific_osname="$(get_legacy_os_name)" || return 1 + + local legacy_download_link=null + if [[ "$runtime" == "dotnet" ]]; then + legacy_download_link="$azure_feed/Runtime/$specific_version/dotnet-$distro_specific_osname-$normalized_architecture.$specific_version.tar.gz" + elif [ -z "$runtime" ]; then + legacy_download_link="$azure_feed/Sdk/$specific_version/dotnet-dev-$distro_specific_osname-$normalized_architecture.$specific_version.tar.gz" + else + return 1 + fi + + echo "$legacy_download_link" + return 0 +} + +get_user_install_path() { + eval $invocation + + if [ ! -z "${DOTNET_INSTALL_DIR:-}" ]; then + echo "$DOTNET_INSTALL_DIR" + else + echo "$HOME/.dotnet" + fi + return 0 +} + +# args: +# install_dir - $1 +resolve_installation_path() { + eval $invocation + + local install_dir=$1 + if [ "$install_dir" = "" ]; then + local user_install_path="$(get_user_install_path)" + say_verbose "resolve_installation_path: user_install_path=$user_install_path" + echo "$user_install_path" + return 0 + fi + + echo "$install_dir" + return 0 +} + +# args: +# relative_or_absolute_path - $1 +get_absolute_path() { + eval $invocation + + local relative_or_absolute_path=$1 + echo "$(cd "$(dirname "$1")" && pwd -P)/$(basename "$1")" + return 0 +} + +# args: +# override - $1 (boolean, true or false) +get_cp_options() { + eval $invocation + + local override="$1" + local override_switch="" + + if [ "$override" = false ]; then + override_switch="-n" + + # create temporary files to check if 'cp -u' is supported + tmp_dir="$(mktemp -d)" + tmp_file="$tmp_dir/testfile" + tmp_file2="$tmp_dir/testfile2" + + touch "$tmp_file" + + # use -u instead of -n if it's available + if cp -u "$tmp_file" "$tmp_file2" 2>/dev/null; then + override_switch="-u" + fi + + # clean up + rm -f "$tmp_file" "$tmp_file2" + rm -rf "$tmp_dir" + fi + + echo "$override_switch" +} + +# args: +# input_files - stdin +# root_path - $1 +# out_path - $2 +# override - $3 +copy_files_or_dirs_from_list() { + eval $invocation + + local root_path="$(remove_trailing_slash "$1")" + local out_path="$(remove_trailing_slash "$2")" + local override="$3" + local override_switch="$(get_cp_options "$override")" + + cat | uniq | while read -r file_path; do + local path="$(remove_beginning_slash "${file_path#$root_path}")" + local target="$out_path/$path" + if [ "$override" = true ] || (! ([ -d "$target" ] || [ -e "$target" ])); then + mkdir -p "$out_path/$(dirname "$path")" + if [ -d "$target" ]; then + rm -rf "$target" + fi + cp -R $override_switch "$root_path/$path" "$target" + fi + done +} + +# args: +# zip_uri - $1 +get_remote_file_size() { + local zip_uri="$1" + + if machine_has "curl"; then + file_size=$(curl -sI "$zip_uri" | grep -i content-length | awk '{ num = $2 + 0; print num }') + elif machine_has "wget"; then + file_size=$(wget --spider --server-response -O /dev/null "$zip_uri" 2>&1 | grep -i 'Content-Length:' | awk '{ num = $2 + 0; print num }') + else + say "Neither curl nor wget is available on this system." + return + fi + + if [ -n "$file_size" ]; then + say "Remote file $zip_uri size is $file_size bytes." + echo "$file_size" + else + say_verbose "Content-Length header was not extracted for $zip_uri." + echo "" + fi +} + +# args: +# zip_path - $1 +# out_path - $2 +# remote_file_size - $3 +extract_dotnet_package() { + eval $invocation + + local zip_path="$1" + local out_path="$2" + local remote_file_size="$3" + + local temp_out_path="$(mktemp -d "$temporary_file_template")" + + local failed=false + tar -xzf "$zip_path" -C "$temp_out_path" > /dev/null || failed=true + + local folders_with_version_regex='^.*/[0-9]+\.[0-9]+[^/]+/' + find "$temp_out_path" -type f | grep -Eo "$folders_with_version_regex" | sort | copy_files_or_dirs_from_list "$temp_out_path" "$out_path" false + find "$temp_out_path" -type f | grep -Ev "$folders_with_version_regex" | copy_files_or_dirs_from_list "$temp_out_path" "$out_path" "$override_non_versioned_files" + + validate_remote_local_file_sizes "$zip_path" "$remote_file_size" + + rm -rf "$temp_out_path" + if [ -z ${keep_zip+x} ]; then + rm -f "$zip_path" && say_verbose "Temporary archive file $zip_path was removed" + fi + + if [ "$failed" = true ]; then + say_err "Extraction failed" + return 1 + fi + return 0 +} + +# args: +# remote_path - $1 +# disable_feed_credential - $2 +get_http_header() +{ + eval $invocation + local remote_path="$1" + local disable_feed_credential="$2" + + local failed=false + local response + if machine_has "curl"; then + get_http_header_curl $remote_path $disable_feed_credential || failed=true + elif machine_has "wget"; then + get_http_header_wget $remote_path $disable_feed_credential || failed=true + else + failed=true + fi + if [ "$failed" = true ]; then + say_verbose "Failed to get HTTP header: '$remote_path'." + return 1 + fi + return 0 +} + +# args: +# remote_path - $1 +# disable_feed_credential - $2 +get_http_header_curl() { + eval $invocation + local remote_path="$1" + local disable_feed_credential="$2" + + remote_path_with_credential="$remote_path" + if [ "$disable_feed_credential" = false ]; then + remote_path_with_credential+="$feed_credential" + fi + + curl_options="-I -sSL --retry 5 --retry-delay 2 --connect-timeout 15 " + curl $curl_options "$remote_path_with_credential" 2>&1 || return 1 + return 0 +} + +# args: +# remote_path - $1 +# disable_feed_credential - $2 +get_http_header_wget() { + eval $invocation + local remote_path="$1" + local disable_feed_credential="$2" + local wget_options="-q -S --spider --tries 5 " + + local wget_options_extra='' + + # Test for options that aren't supported on all wget implementations. + if [[ $(wget -h 2>&1 | grep -E 'waitretry|connect-timeout') ]]; then + wget_options_extra="--waitretry 2 --connect-timeout 15 " + else + say "wget extra options are unavailable for this environment" + fi + + remote_path_with_credential="$remote_path" + if [ "$disable_feed_credential" = false ]; then + remote_path_with_credential+="$feed_credential" + fi + + wget $wget_options $wget_options_extra "$remote_path_with_credential" 2>&1 + + return $? +} + +# args: +# remote_path - $1 +# [out_path] - $2 - stdout if not provided +download() { + eval $invocation + + local remote_path="$1" + local out_path="${2:-}" + + if [[ "$remote_path" != "http"* ]]; then + cp "$remote_path" "$out_path" + return $? + fi + + local failed=false + local attempts=0 + while [ $attempts -lt 3 ]; do + attempts=$((attempts+1)) + failed=false + if machine_has "curl"; then + downloadcurl "$remote_path" "$out_path" || failed=true + elif machine_has "wget"; then + downloadwget "$remote_path" "$out_path" || failed=true + else + say_err "Missing dependency: neither curl nor wget was found." + exit 1 + fi + + if [ "$failed" = false ] || [ $attempts -ge 3 ] || { [ -n "${http_code-}" ] && [ "${http_code}" = "404" ]; }; then + break + fi + + say "Download attempt #$attempts has failed: ${http_code-} ${download_error_msg-}" + say "Attempt #$((attempts+1)) will start in $((attempts*10)) seconds." + sleep $((attempts*10)) + done + + if [ "$failed" = true ]; then + say_verbose "Download failed: $remote_path" + return 1 + fi + return 0 +} + +# Updates global variables $http_code and $download_error_msg +downloadcurl() { + eval $invocation + unset http_code + unset download_error_msg + local remote_path="$1" + local out_path="${2:-}" + # Append feed_credential as late as possible before calling curl to avoid logging feed_credential + # Avoid passing URI with credentials to functions: note, most of them echoing parameters of invocation in verbose output. + local remote_path_with_credential="${remote_path}${feed_credential}" + local curl_options="--retry 20 --retry-delay 2 --connect-timeout 15 -sSL -f --create-dirs " + local curl_exit_code=0; + if [ -z "$out_path" ]; then + curl_output=$(curl $curl_options "$remote_path_with_credential" 2>&1) + curl_exit_code=$? + echo "$curl_output" + else + curl_output=$(curl $curl_options -o "$out_path" "$remote_path_with_credential" 2>&1) + curl_exit_code=$? + fi + + # Regression in curl causes curl with --retry to return a 0 exit code even when it fails to download a file - https://github.com/curl/curl/issues/17554 + if [ $curl_exit_code -eq 0 ] && echo "$curl_output" | grep -q "^curl: ([0-9]*) "; then + curl_exit_code=$(echo "$curl_output" | sed 's/curl: (\([0-9]*\)).*/\1/') + fi + + if [ $curl_exit_code -gt 0 ]; then + download_error_msg="Unable to download $remote_path." + # Check for curl timeout codes + if [[ $curl_exit_code == 7 || $curl_exit_code == 28 ]]; then + download_error_msg+=" Failed to reach the server: connection timeout." + else + local disable_feed_credential=false + local response=$(get_http_header_curl $remote_path $disable_feed_credential) + http_code=$( echo "$response" | awk '/^HTTP/{print $2}' | tail -1 ) + if [[ ! -z $http_code && $http_code != 2* ]]; then + download_error_msg+=" Returned HTTP status code: $http_code." + fi + fi + say_verbose "$download_error_msg" + return 1 + fi + return 0 +} + + +# Updates global variables $http_code and $download_error_msg +downloadwget() { + eval $invocation + unset http_code + unset download_error_msg + local remote_path="$1" + local out_path="${2:-}" + # Append feed_credential as late as possible before calling wget to avoid logging feed_credential + local remote_path_with_credential="${remote_path}${feed_credential}" + local wget_options="--tries 20 " + + local wget_options_extra='' + local wget_result='' + + # Test for options that aren't supported on all wget implementations. + if [[ $(wget -h 2>&1 | grep -E 'waitretry|connect-timeout') ]]; then + wget_options_extra="--waitretry 2 --connect-timeout 15 " + else + say "wget extra options are unavailable for this environment" + fi + + if [ -z "$out_path" ]; then + wget -q $wget_options $wget_options_extra -O - "$remote_path_with_credential" 2>&1 + wget_result=$? + else + wget $wget_options $wget_options_extra -O "$out_path" "$remote_path_with_credential" 2>&1 + wget_result=$? + fi + + if [[ $wget_result != 0 ]]; then + local disable_feed_credential=false + local response=$(get_http_header_wget $remote_path $disable_feed_credential) + http_code=$( echo "$response" | awk '/^ HTTP/{print $2}' | tail -1 ) + download_error_msg="Unable to download $remote_path." + if [[ ! -z $http_code && $http_code != 2* ]]; then + download_error_msg+=" Returned HTTP status code: $http_code." + # wget exit code 4 stands for network-issue + elif [[ $wget_result == 4 ]]; then + download_error_msg+=" Failed to reach the server: connection timeout." + fi + say_verbose "$download_error_msg" + return 1 + fi + + return 0 +} + +get_download_link_from_aka_ms() { + eval $invocation + + #quality is not supported for LTS or STS channel + #STS maps to current + if [[ ! -z "$normalized_quality" && ("$normalized_channel" == "LTS" || "$normalized_channel" == "STS") ]]; then + normalized_quality="" + say_warning "Specifying quality for STS or LTS channel is not supported, the quality will be ignored." + fi + + say_verbose "Retrieving primary payload URL from aka.ms for channel: '$normalized_channel', quality: '$normalized_quality', product: '$normalized_product', os: '$normalized_os', architecture: '$normalized_architecture'." + + #construct aka.ms link + aka_ms_link="https://aka.ms/dotnet" + if [ "$internal" = true ]; then + aka_ms_link="$aka_ms_link/internal" + fi + aka_ms_link="$aka_ms_link/$normalized_channel" + if [[ ! -z "$normalized_quality" ]]; then + aka_ms_link="$aka_ms_link/$normalized_quality" + fi + aka_ms_link="$aka_ms_link/$normalized_product-$normalized_os-$normalized_architecture.tar.gz" + say_verbose "Constructed aka.ms link: '$aka_ms_link'." + + #get HTTP response + #do not pass credentials as a part of the $aka_ms_link and do not apply credentials in the get_http_header function + #otherwise the redirect link would have credentials as well + #it would result in applying credentials twice to the resulting link and thus breaking it, and in echoing credentials to the output as a part of redirect link + disable_feed_credential=true + response="$(get_http_header $aka_ms_link $disable_feed_credential)" + + say_verbose "Received response: $response" + # Get results of all the redirects. + http_codes=$( echo "$response" | awk '$1 ~ /^HTTP/ {print $2}' ) + # They all need to be 301, otherwise some links are broken (except for the last, which is not a redirect but 200 or 404). + broken_redirects=$( echo "$http_codes" | sed '$d' | grep -v '301' ) + # The response may end without final code 2xx/4xx/5xx somehow, e.g. network restrictions on www.bing.com causes redirecting to bing.com fails with connection refused. + # In this case it should not exclude the last. + last_http_code=$( echo "$http_codes" | tail -n 1 ) + if ! [[ $last_http_code =~ ^(2|4|5)[0-9][0-9]$ ]]; then + broken_redirects=$( echo "$http_codes" | grep -v '301' ) + fi + + # All HTTP codes are 301 (Moved Permanently), the redirect link exists. + if [[ -z "$broken_redirects" ]]; then + aka_ms_download_link=$( echo "$response" | awk '$1 ~ /^Location/{print $2}' | tail -1 | tr -d '\r') + + if [[ -z "$aka_ms_download_link" ]]; then + say_verbose "The aka.ms link '$aka_ms_link' is not valid: failed to get redirect location." + return 1 + fi + + say_verbose "The redirect location retrieved: '$aka_ms_download_link'." + return 0 + else + say_verbose "The aka.ms link '$aka_ms_link' is not valid: received HTTP code: $(echo "$broken_redirects" | paste -sd "," -)." + return 1 + fi +} + +get_feeds_to_use() +{ + feeds=( + "https://builds.dotnet.microsoft.com/dotnet" + "https://ci.dot.net/public" + ) + + if [[ -n "$azure_feed" ]]; then + feeds=("$azure_feed") + fi + + if [[ -n "$uncached_feed" ]]; then + feeds=("$uncached_feed") + fi +} + +# THIS FUNCTION MAY EXIT (if the determined version is already installed). +generate_download_links() { + + download_links=() + specific_versions=() + effective_versions=() + link_types=() + + # If generate_akams_links returns false, no fallback to old links. Just terminate. + # This function may also 'exit' (if the determined version is already installed). + generate_akams_links || return + + # Check other feeds only if we haven't been able to find an aka.ms link. + if [[ "${#download_links[@]}" -lt 1 ]]; then + for feed in ${feeds[@]} + do + # generate_regular_links may also 'exit' (if the determined version is already installed). + generate_regular_links $feed || return + done + fi + + if [[ "${#download_links[@]}" -eq 0 ]]; then + say_err "Failed to resolve the exact version number." + return 1 + fi + + say_verbose "Generated ${#download_links[@]} links." + for link_index in ${!download_links[@]} + do + say_verbose "Link $link_index: ${link_types[$link_index]}, ${effective_versions[$link_index]}, ${download_links[$link_index]}" + done +} + +# THIS FUNCTION MAY EXIT (if the determined version is already installed). +generate_akams_links() { + local valid_aka_ms_link=true; + + normalized_version="$(to_lowercase "$version")" + if [[ "$normalized_version" != "latest" ]] && [ -n "$normalized_quality" ]; then + say_err "Quality and Version options are not allowed to be specified simultaneously. See https://learn.microsoft.com/dotnet/core/tools/dotnet-install-script#options for details." + return 1 + fi + + if [[ -n "$json_file" || "$normalized_version" != "latest" ]]; then + # aka.ms links are not needed when exact version is specified via command or json file + return + fi + + get_download_link_from_aka_ms || valid_aka_ms_link=false + + if [[ "$valid_aka_ms_link" == true ]]; then + say_verbose "Retrieved primary payload URL from aka.ms link: '$aka_ms_download_link'." + say_verbose "Downloading using legacy url will not be attempted." + + download_link=$aka_ms_download_link + + #get version from the path + IFS='/' + read -ra pathElems <<< "$download_link" + count=${#pathElems[@]} + specific_version="${pathElems[count-2]}" + unset IFS; + say_verbose "Version: '$specific_version'." + + #Retrieve effective version + effective_version="$(get_specific_product_version "$azure_feed" "$specific_version" "$download_link")" + + # Add link info to arrays + download_links+=($download_link) + specific_versions+=($specific_version) + effective_versions+=($effective_version) + link_types+=("aka.ms") + + # Check if the SDK version is already installed. + if [[ "$dry_run" != true ]] && is_dotnet_package_installed "$install_root" "$asset_relative_path" "$effective_version"; then + say "$asset_name with version '$effective_version' is already installed." + exit 0 + fi + + return 0 + fi + + # if quality is specified - exit with error - there is no fallback approach + if [ ! -z "$normalized_quality" ]; then + say_err "Failed to locate the latest version in the channel '$normalized_channel' with '$normalized_quality' quality for '$normalized_product', os: '$normalized_os', architecture: '$normalized_architecture'." + say_err "Refer to: https://aka.ms/dotnet-os-lifecycle for information on .NET Core support." + return 1 + fi + say_verbose "Falling back to latest.version file approach." +} + +# THIS FUNCTION MAY EXIT (if the determined version is already installed) +# args: +# feed - $1 +generate_regular_links() { + local feed="$1" + local valid_legacy_download_link=true + + specific_version=$(get_specific_version_from_version "$feed" "$channel" "$normalized_architecture" "$version" "$json_file") || specific_version='0' + + if [[ "$specific_version" == '0' ]]; then + say_verbose "Failed to resolve the specific version number using feed '$feed'" + return + fi + + effective_version="$(get_specific_product_version "$feed" "$specific_version")" + say_verbose "specific_version=$specific_version" + + download_link="$(construct_download_link "$feed" "$channel" "$normalized_architecture" "$specific_version" "$normalized_os")" + say_verbose "Constructed primary named payload URL: $download_link" + + # Add link info to arrays + download_links+=($download_link) + specific_versions+=($specific_version) + effective_versions+=($effective_version) + link_types+=("primary") + + legacy_download_link="$(construct_legacy_download_link "$feed" "$channel" "$normalized_architecture" "$specific_version")" || valid_legacy_download_link=false + + if [ "$valid_legacy_download_link" = true ]; then + say_verbose "Constructed legacy named payload URL: $legacy_download_link" + + download_links+=($legacy_download_link) + specific_versions+=($specific_version) + effective_versions+=($effective_version) + link_types+=("legacy") + else + legacy_download_link="" + say_verbose "Could not construct a legacy_download_link; omitting..." + fi + + # Check if the SDK version is already installed. + if [[ "$dry_run" != true ]] && is_dotnet_package_installed "$install_root" "$asset_relative_path" "$effective_version"; then + say "$asset_name with version '$effective_version' is already installed." + exit 0 + fi +} + +print_dry_run() { + + say "Payload URLs:" + + for link_index in "${!download_links[@]}" + do + say "URL #$link_index - ${link_types[$link_index]}: ${download_links[$link_index]}" + done + + resolved_version=${specific_versions[0]} + repeatable_command="./$script_name --version "\""$resolved_version"\"" --install-dir "\""$install_root"\"" --architecture "\""$normalized_architecture"\"" --os "\""$normalized_os"\""" + + if [ ! -z "$normalized_quality" ]; then + repeatable_command+=" --quality "\""$normalized_quality"\""" + fi + + if [[ "$runtime" == "dotnet" ]]; then + repeatable_command+=" --runtime "\""dotnet"\""" + elif [[ "$runtime" == "aspnetcore" ]]; then + repeatable_command+=" --runtime "\""aspnetcore"\""" + fi + + repeatable_command+="$non_dynamic_parameters" + + if [ -n "$feed_credential" ]; then + repeatable_command+=" --feed-credential "\"""\""" + fi + + say "Repeatable invocation: $repeatable_command" +} + +calculate_vars() { + eval $invocation + + script_name=$(basename "$0") + normalized_architecture="$(get_normalized_architecture_from_architecture "$architecture")" + say_verbose "Normalized architecture: '$normalized_architecture'." + normalized_os="$(get_normalized_os "$user_defined_os")" + say_verbose "Normalized OS: '$normalized_os'." + normalized_quality="$(get_normalized_quality "$quality")" + say_verbose "Normalized quality: '$normalized_quality'." + normalized_channel="$(get_normalized_channel "$channel")" + say_verbose "Normalized channel: '$normalized_channel'." + normalized_product="$(get_normalized_product "$runtime")" + say_verbose "Normalized product: '$normalized_product'." + install_root="$(resolve_installation_path "$install_dir")" + say_verbose "InstallRoot: '$install_root'." + + normalized_architecture="$(get_normalized_architecture_for_specific_sdk_version "$version" "$normalized_channel" "$normalized_architecture")" + + if [[ "$runtime" == "dotnet" ]]; then + asset_relative_path="shared/Microsoft.NETCore.App" + asset_name=".NET Core Runtime" + elif [[ "$runtime" == "aspnetcore" ]]; then + asset_relative_path="shared/Microsoft.AspNetCore.App" + asset_name="ASP.NET Core Runtime" + elif [ -z "$runtime" ]; then + asset_relative_path="sdk" + asset_name=".NET Core SDK" + fi + + get_feeds_to_use +} + +install_dotnet() { + eval $invocation + local download_failed=false + local download_completed=false + local remote_file_size=0 + + mkdir -p "$install_root" + zip_path="${zip_path:-$(mktemp "$temporary_file_template")}" + say_verbose "Archive path: $zip_path" + + for link_index in "${!download_links[@]}" + do + download_link="${download_links[$link_index]}" + specific_version="${specific_versions[$link_index]}" + effective_version="${effective_versions[$link_index]}" + link_type="${link_types[$link_index]}" + + say "Attempting to download using $link_type link $download_link" + + # The download function will set variables $http_code and $download_error_msg in case of failure. + download_failed=false + download "$download_link" "$zip_path" 2>&1 || download_failed=true + + if [ "$download_failed" = true ]; then + case ${http_code-} in + 404) + say "The resource at $link_type link '$download_link' is not available." + ;; + *) + say "Failed to download $link_type link '$download_link': ${http_code-} ${download_error_msg-}" + ;; + esac + rm -f "$zip_path" 2>&1 && say_verbose "Temporary archive file $zip_path was removed" + else + download_completed=true + break + fi + done + + if [[ "$download_completed" == false ]]; then + say_err "Could not find \`$asset_name\` with version = $specific_version" + say_err "Refer to: https://aka.ms/dotnet-os-lifecycle for information on .NET Core support" + return 1 + fi + + remote_file_size="$(get_remote_file_size "$download_link")" + + say "Extracting archive from $download_link" + extract_dotnet_package "$zip_path" "$install_root" "$remote_file_size" || return 1 + + # Check if the SDK version is installed; if not, fail the installation. + # if the version contains "RTM" or "servicing"; check if a 'release-type' SDK version is installed. + if [[ $specific_version == *"rtm"* || $specific_version == *"servicing"* ]]; then + IFS='-' + read -ra verArr <<< "$specific_version" + release_version="${verArr[0]}" + unset IFS; + say_verbose "Checking installation: version = $release_version" + if is_dotnet_package_installed "$install_root" "$asset_relative_path" "$release_version"; then + say "Installed version is $effective_version" + return 0 + fi + fi + + # Check if the standard SDK version is installed. + say_verbose "Checking installation: version = $effective_version" + if is_dotnet_package_installed "$install_root" "$asset_relative_path" "$effective_version"; then + say "Installed version is $effective_version" + return 0 + fi + + # Version verification failed. More likely something is wrong either with the downloaded content or with the verification algorithm. + say_err "Failed to verify the version of installed \`$asset_name\`.\nInstallation source: $download_link.\nInstallation location: $install_root.\nReport the bug at https://github.com/dotnet/install-scripts/issues." + say_err "\`$asset_name\` with version = $effective_version failed to install with an error." + return 1 +} + +args=("$@") + +local_version_file_relative_path="/.version" +bin_folder_relative_path="" +temporary_file_template="${TMPDIR:-/tmp}/dotnet.XXXXXXXXX" + +channel="LTS" +version="Latest" +json_file="" +install_dir="" +architecture="" +dry_run=false +no_path=false +azure_feed="" +uncached_feed="" +feed_credential="" +verbose=false +runtime="" +runtime_id="" +quality="" +internal=false +override_non_versioned_files=true +non_dynamic_parameters="" +user_defined_os="" + +while [ $# -ne 0 ] +do + name="$1" + case "$name" in + -c|--channel|-[Cc]hannel) + shift + channel="$1" + ;; + -v|--version|-[Vv]ersion) + shift + version="$1" + ;; + -q|--quality|-[Qq]uality) + shift + quality="$1" + ;; + --internal|-[Ii]nternal) + internal=true + non_dynamic_parameters+=" $name" + ;; + -i|--install-dir|-[Ii]nstall[Dd]ir) + shift + install_dir="$1" + ;; + --arch|--architecture|-[Aa]rch|-[Aa]rchitecture) + shift + architecture="$1" + ;; + --os|-[Oo][SS]) + shift + user_defined_os="$1" + ;; + --shared-runtime|-[Ss]hared[Rr]untime) + say_warning "The --shared-runtime flag is obsolete and may be removed in a future version of this script. The recommended usage is to specify '--runtime dotnet'." + if [ -z "$runtime" ]; then + runtime="dotnet" + fi + ;; + --runtime|-[Rr]untime) + shift + runtime="$1" + if [[ "$runtime" != "dotnet" ]] && [[ "$runtime" != "aspnetcore" ]]; then + say_err "Unsupported value for --runtime: '$1'. Valid values are 'dotnet' and 'aspnetcore'." + if [[ "$runtime" == "windowsdesktop" ]]; then + say_err "WindowsDesktop archives are manufactured for Windows platforms only." + fi + exit 1 + fi + ;; + --dry-run|-[Dd]ry[Rr]un) + dry_run=true + ;; + --no-path|-[Nn]o[Pp]ath) + no_path=true + non_dynamic_parameters+=" $name" + ;; + --verbose|-[Vv]erbose) + verbose=true + non_dynamic_parameters+=" $name" + ;; + --azure-feed|-[Aa]zure[Ff]eed) + shift + azure_feed="$1" + non_dynamic_parameters+=" $name "\""$1"\""" + ;; + --uncached-feed|-[Uu]ncached[Ff]eed) + shift + uncached_feed="$1" + non_dynamic_parameters+=" $name "\""$1"\""" + ;; + --feed-credential|-[Ff]eed[Cc]redential) + shift + feed_credential="$1" + #feed_credential should start with "?", for it to be added to the end of the link. + #adding "?" at the beginning of the feed_credential if needed. + [[ -z "$(echo $feed_credential)" ]] || [[ $feed_credential == \?* ]] || feed_credential="?$feed_credential" + ;; + --runtime-id|-[Rr]untime[Ii]d) + shift + runtime_id="$1" + non_dynamic_parameters+=" $name "\""$1"\""" + say_warning "Use of --runtime-id is obsolete and should be limited to the versions below 2.1. To override architecture, use --architecture option instead. To override OS, use --os option instead." + ;; + --jsonfile|-[Jj][Ss]on[Ff]ile) + shift + json_file="$1" + ;; + --skip-non-versioned-files|-[Ss]kip[Nn]on[Vv]ersioned[Ff]iles) + override_non_versioned_files=false + non_dynamic_parameters+=" $name" + ;; + --keep-zip|-[Kk]eep[Zz]ip) + keep_zip=true + non_dynamic_parameters+=" $name" + ;; + --zip-path|-[Zz]ip[Pp]ath) + shift + zip_path="$1" + ;; + -?|--?|-h|--help|-[Hh]elp) + script_name="dotnet-install.sh" + echo ".NET Tools Installer" + echo "Usage:" + echo " # Install a .NET SDK of a given Quality from a given Channel" + echo " $script_name [-c|--channel ] [-q|--quality ]" + echo " # Install a .NET SDK of a specific public version" + echo " $script_name [-v|--version ]" + echo " $script_name -h|-?|--help" + echo "" + echo "$script_name is a simple command line interface for obtaining dotnet cli." + echo " Note that the intended use of this script is for Continuous Integration (CI) scenarios, where:" + echo " - The SDK needs to be installed without user interaction and without admin rights." + echo " - The SDK installation doesn't need to persist across multiple CI runs." + echo " To set up a development environment or to run apps, use installers rather than this script. Visit https://dotnet.microsoft.com/download to get the installer." + echo "" + echo "Options:" + echo " -c,--channel Download from the channel specified, Defaults to \`$channel\`." + echo " -Channel" + echo " Possible values:" + echo " - STS - the most recent Standard Term Support release" + echo " - LTS - the most recent Long Term Support release" + echo " - 2-part version in a format A.B - represents a specific release" + echo " examples: 2.0; 1.0" + echo " - 3-part version in a format A.B.Cxx - represents a specific SDK release" + echo " examples: 5.0.1xx, 5.0.2xx." + echo " Supported since 5.0 release" + echo " Warning: Value 'Current' is deprecated for the Channel parameter. Use 'STS' instead." + echo " Note: The version parameter overrides the channel parameter when any version other than 'latest' is used." + echo " -v,--version Use specific VERSION, Defaults to \`$version\`." + echo " -Version" + echo " Possible values:" + echo " - latest - the latest build on specific channel" + echo " - 3-part version in a format A.B.C - represents specific version of build" + echo " examples: 2.0.0-preview2-006120; 1.1.0" + echo " -q,--quality Download the latest build of specified quality in the channel." + echo " -Quality" + echo " The possible values are: daily, preview, GA." + echo " Works only in combination with channel. Not applicable for STS and LTS channels and will be ignored if those channels are used." + echo " For SDK use channel in A.B.Cxx format. Using quality for SDK together with channel in A.B format is not supported." + echo " Supported since 5.0 release." + echo " Note: The version parameter overrides the channel parameter when any version other than 'latest' is used, and therefore overrides the quality." + echo " --internal,-Internal Download internal builds. Requires providing credentials via --feed-credential parameter." + echo " --feed-credential Token to access Azure feed. Used as a query string to append to the Azure feed." + echo " -FeedCredential This parameter typically is not specified." + echo " -i,--install-dir Install under specified location (see Install Location below)" + echo " -InstallDir" + echo " --architecture Architecture of dotnet binaries to be installed, Defaults to \`$architecture\`." + echo " --arch,-Architecture,-Arch" + echo " Possible values: x64, arm, arm64, s390x, ppc64le and loongarch64" + echo " --os Specifies operating system to be used when selecting the installer." + echo " Overrides the OS determination approach used by the script. Supported values: osx, linux, linux-musl, freebsd, rhel.6." + echo " In case any other value is provided, the platform will be determined by the script based on machine configuration." + echo " Not supported for legacy links. Use --runtime-id to specify platform for legacy links." + echo " Refer to: https://aka.ms/dotnet-os-lifecycle for more information." + echo " --runtime Installs a shared runtime only, without the SDK." + echo " -Runtime" + echo " Possible values:" + echo " - dotnet - the Microsoft.NETCore.App shared runtime" + echo " - aspnetcore - the Microsoft.AspNetCore.App shared runtime" + echo " --dry-run,-DryRun Do not perform installation. Display download link." + echo " --no-path, -NoPath Do not set PATH for the current process." + echo " --verbose,-Verbose Display diagnostics information." + echo " --azure-feed,-AzureFeed For internal use only." + echo " Allows using a different storage to download SDK archives from." + echo " --uncached-feed,-UncachedFeed For internal use only." + echo " Allows using a different storage to download SDK archives from." + echo " --skip-non-versioned-files Skips non-versioned files if they already exist, such as the dotnet executable." + echo " -SkipNonVersionedFiles" + echo " --jsonfile Determines the SDK version from a user specified global.json file." + echo " Note: global.json must have a value for 'SDK:Version'" + echo " --keep-zip,-KeepZip If set, downloaded file is kept." + echo " --zip-path, -ZipPath If set, downloaded file is stored at the specified path." + echo " -?,--?,-h,--help,-Help Shows this help message" + echo "" + echo "Install Location:" + echo " Location is chosen in following order:" + echo " - --install-dir option" + echo " - Environmental variable DOTNET_INSTALL_DIR" + echo " - $HOME/.dotnet" + exit 0 + ;; + *) + say_err "Unknown argument \`$name\`" + exit 1 + ;; + esac + + shift +done + +say_verbose "Note that the intended use of this script is for Continuous Integration (CI) scenarios, where:" +say_verbose "- The SDK needs to be installed without user interaction and without admin rights." +say_verbose "- The SDK installation doesn't need to persist across multiple CI runs." +say_verbose "To set up a development environment or to run apps, use installers rather than this script. Visit https://dotnet.microsoft.com/download to get the installer.\n" + +if [ "$internal" = true ] && [ -z "$(echo $feed_credential)" ]; then + message="Provide credentials via --feed-credential parameter." + if [ "$dry_run" = true ]; then + say_warning "$message" + else + say_err "$message" + exit 1 + fi +fi + +check_min_reqs +calculate_vars +# generate_regular_links call below will 'exit' if the determined version is already installed. +generate_download_links + +if [[ "$dry_run" = true ]]; then + print_dry_run + exit 0 +fi + +install_dotnet + +bin_path="$(get_absolute_path "$(combine_paths "$install_root" "$bin_folder_relative_path")")" +if [ "$no_path" = false ]; then + say "Adding to current process PATH: \`$bin_path\`. Note: This change will be visible only when sourcing script." + export PATH="$bin_path":"$PATH" +else + say "Binaries of dotnet can be found in $bin_path" +fi + +say "Note that the script does not resolve dependencies during installation." +say "To check the list of dependencies, go to https://learn.microsoft.com/dotnet/core/install, select your operating system and check the \"Dependencies\" section." +say "Installation finished successfully." diff --git a/examples/run_example.sh b/examples/run_example.sh new file mode 100755 index 0000000..e8466c3 --- /dev/null +++ b/examples/run_example.sh @@ -0,0 +1,117 @@ +#!/bin/bash + +# Script để build và chạy C# example + +set -e + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PROJECT_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)" +BUILD_DIR="$PROJECT_ROOT/build" +LIB_DIR="$BUILD_DIR/src/APIs/c_api" +EXAMPLE_DIR="$SCRIPT_DIR" + +echo "==========================================" +echo "Building and Running C# Navigation Example" +echo "==========================================" +echo "" +echo "📁 Paths:" +echo " SCRIPT_DIR: $SCRIPT_DIR" +echo " PROJECT_ROOT: $PROJECT_ROOT" +echo " BUILD_DIR: $BUILD_DIR" +echo " LIB_DIR: $LIB_DIR" +echo " EXAMPLE_DIR: $EXAMPLE_DIR" +echo "" + + +echo "Building C API library..." +cd "$BUILD_DIR" +cmake .. +make +echo "Library built successfully!" + + +# Kiểm tra dotnet có được cài đặt không +if ! command -v dotnet &> /dev/null; then + echo "==========================================" + echo "WARNING: .NET SDK not found!" + echo "==========================================" + echo "To run the C# example, please install .NET SDK:" + echo "" + echo " Ubuntu/Debian:" + echo " wget https://dot.net/v1/dotnet-install.sh -O dotnet-install.sh" + echo " chmod +x dotnet-install.sh" + echo " ./dotnet-install.sh --channel 6.0" + echo " export PATH=\$PATH:\$HOME/.dotnet" + echo "" + echo " Or visit: https://dotnet.microsoft.com/download" + echo "" + echo "Skipping C# example. Library is available at:" + echo " $LIB_DIR/libnav_c_api.so" + echo "==========================================" + exit 0 +fi + +# Bước 2: Tạo C# project nếu chưa có +if [ ! -d "$EXAMPLE_DIR/NavigationExample" ]; then + echo "Creating C# project..." + cd "$EXAMPLE_DIR" + dotnet new console -n NavigationExample + cd NavigationExample + + # Tạo .csproj file + cat > NavigationExample.csproj << 'EOF' + + + Exe + net6.0 + linux-x64 + true + + + + PreserveNewest + + + +EOF + + echo "C# project created!" +else + echo "C# project already exists" +fi + +# Luôn copy source code mới nhất (cập nhật file nếu đã có) +cd "$EXAMPLE_DIR/NavigationExample" +echo "Updating Program.cs from CSharpExample.cs..." +cp ../CSharpExample.cs Program.cs + +# Bước 3: Copy library +echo "Copying library..." +cp "$LIB_DIR/libnav_c_api.so" . + +# Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies +export LD_LIBRARY_PATH="$LIB_DIR:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH" + +# Bước 5: Build và chạy +echo "Building C# project..." +dotnet build + +echo "" +echo "Running example..." +echo "==========================================" +echo "LD_LIBRARY_PATH includes:" +echo "$LD_LIBRARY_PATH" | tr ':' '\n' | head -5 +echo "..." +echo "==========================================" +dotnet run + +echo "==========================================" +echo "Done!" + diff --git a/setup_catkin_workspace.sh b/setup_catkin_workspace.sh new file mode 100755 index 0000000..ee22c71 --- /dev/null +++ b/setup_catkin_workspace.sh @@ -0,0 +1,116 @@ +#!/bin/bash +# ======================================================== +# Script to setup Catkin workspace for pnkx_nav_core +# ======================================================== + +set -e + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +WORKSPACE_NAME="pnkx_nav_catkin_ws" +WORKSPACE_DIR="${SCRIPT_DIR}/../${WORKSPACE_NAME}" + +echo "==========================================" +echo "Setting up Catkin workspace for pnkx_nav_core" +echo "==========================================" +echo "" + +# Create workspace directory +echo "Creating workspace directory: ${WORKSPACE_DIR}" +mkdir -p "${WORKSPACE_DIR}/src" + +# Check if src directory already has packages +if [ -d "${WORKSPACE_DIR}/src" ] && [ "$(ls -A ${WORKSPACE_DIR}/src 2>/dev/null)" ]; then + echo "Warning: ${WORKSPACE_DIR}/src already contains packages" + read -p "Do you want to continue? (y/n) " -n 1 -r + echo + if [[ ! $REPLY =~ ^[Yy]$ ]]; then + echo "Aborted." + exit 1 + fi +fi + +# Create symlinks to packages that have package.xml +echo "" +echo "Creating symlinks to packages with package.xml..." + +# Find all packages with package.xml in the source tree +PACKAGES=( + "src/Libraries/tf3" + "src/Libraries/nav_2d_utils" + "src/Navigations/Libraries/nav_grid" + "src/Navigations/Cores/nav_core" + "src/Navigations/Cores/nav_core2" + "src/Navigations/Cores/nav_core_adapter" + "src/Navigations/Cores/move_base_core" + "src/Navigations/Cores/nav_core_adapter" + "src/Packages/move_base" + "src/Navigations/Packages/move_base" +) + +for PKG_PATH in "${PACKAGES[@]}"; do + FULL_PATH="${SCRIPT_DIR}/${PKG_PATH}" + if [ -f "${FULL_PATH}/package.xml" ]; then + PKG_NAME=$(basename "${PKG_PATH}") + TARGET="${WORKSPACE_DIR}/src/${PKG_NAME}" + + if [ -e "${TARGET}" ]; then + echo " Warning: ${TARGET} already exists, skipping..." + else + echo " Linking: ${PKG_NAME} -> ${PKG_PATH}" + ln -s "${FULL_PATH}" "${TARGET}" + fi + else + echo " Warning: ${PKG_PATH}/package.xml not found, skipping..." + fi +done + +# Create a README in the workspace +cat > "${WORKSPACE_DIR}/README.md" << EOF +# Catkin Workspace for pnkx_nav_core + +This is a Catkin workspace containing ROS-compatible packages from pnkx_nav_core. + +## Structure + +\`\`\` +${WORKSPACE_NAME}/ +├── src/ # Source packages (symlinked from pnkx_nav_core) +└── build/ # Build directory (created by catkin_make) +\`\`\` + +## Building + +\`\`\`bash +cd ${WORKSPACE_DIR} +catkin_make +\`\`\` + +## Source the workspace + +After building, source the setup file: + +\`\`\`bash +source devel/setup.bash +\`\`\` + +## Note + +Packages in \`src/\` are symlinked from the main pnkx_nav_core project. +To add more packages, create symlinks in \`src/\` pointing to packages +that have both \`CMakeLists.txt\` and \`package.xml\`. +EOF + +echo "" +echo "==========================================" +echo "Workspace setup complete!" +echo "==========================================" +echo "" +echo "Next steps:" +echo " 1. cd ${WORKSPACE_DIR}" +echo " 2. catkin_make" +echo " 3. source devel/setup.bash" +echo "" +echo "Note: Make sure all dependencies are built in the main project first" +echo " using standalone CMake before building with catkin." +echo "" + diff --git a/src/APIs/c_api/CMakeLists.txt b/src/APIs/c_api/CMakeLists.txt new file mode 100644 index 0000000..ce964f8 --- /dev/null +++ b/src/APIs/c_api/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.10) +project(nav_c_api VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# Dependencies +set(PACKAGES_DIR + move_base_core + move_base + tf3 + robot_time + geometry_msgs +) + +# Thư mục include +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +# Tìm tất cả file source +file(GLOB SOURCES "src/*.cpp") +file(GLOB HEADERS "include/*.h") + +# Tạo thư viện shared (.so) +add_library(nav_c_api SHARED ${SOURCES} ${HEADERS}) + +# Link libraries +target_link_libraries(nav_c_api + PUBLIC + ${PACKAGES_DIR} +) + +# Set include directories +target_include_directories(nav_c_api + PUBLIC + $ + $ +) + +# Cài đặt header files +install(DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" +) + +# Cài đặt library +install(TARGETS nav_c_api + EXPORT nav_c_api-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +# Export targets +install(EXPORT nav_c_api-targets + FILE nav_c_api-targets.cmake + DESTINATION lib/cmake/nav_c_api +) + +# Tùy chọn build +option(BUILD_SHARED_LIBS "Build shared libraries" ON) +option(BUILD_TESTS "Build test programs" OFF) + +# Flags biên dịch +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "=================================") + diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h new file mode 100644 index 0000000..42c6024 --- /dev/null +++ b/src/APIs/c_api/include/nav_c_api.h @@ -0,0 +1,349 @@ +#ifndef NAVIGATION_C_API_H +#define NAVIGATION_C_API_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +// Forward declarations +typedef void* NavigationHandle; +typedef void* TFListenerHandle; + +// ============================================================================ +// Enums +// ============================================================================ + +/** + * @brief Navigation states, including planning and controller status + */ +typedef enum { + NAV_STATE_PENDING = 0, + NAV_STATE_ACTIVE = 1, + NAV_STATE_PREEMPTED = 2, + NAV_STATE_SUCCEEDED = 3, + NAV_STATE_ABORTED = 4, + NAV_STATE_REJECTED = 5, + NAV_STATE_PREEMPTING = 6, + NAV_STATE_RECALLING = 7, + NAV_STATE_RECALLED = 8, + NAV_STATE_LOST = 9, + NAV_STATE_PLANNING = 10, + NAV_STATE_CONTROLLING = 11, + NAV_STATE_CLEARING = 12, + NAV_STATE_PAUSED = 13 +} NavigationState; + +// ============================================================================ +// Structures +// ============================================================================ + +/** + * @brief Point structure (x, y, z) + */ +typedef struct { + double x; + double y; + double z; +} Point; + +/** + * @brief Pose2D structure (x, y, theta) + */ +typedef struct { + double x; + double y; + double theta; +} Pose2D; + +/** + * @brief Quaternion structure + */ +typedef struct { + double x; + double y; + double z; + double w; +} Quaternion; + +/** + * @brief Position structure + */ +typedef struct { + double x; + double y; + double z; +} Position; + +/** + * @brief Pose structure + */ +typedef struct { + Position position; + Quaternion orientation; +} Pose; + +/** + * @brief Header structure + */ +typedef struct { + uint32_t seq; + int64_t sec; + uint32_t nsec; + char* frame_id; +} Header; + +/** + * @brief PoseStamped structure + */ +typedef struct { + Header header; + Pose pose; +} PoseStamped; + +/** + * @brief Vector3 structure + */ +typedef struct { + double x; + double y; + double z; +} Vector3; + +/** + * @brief Navigation feedback structure + */ +typedef struct { + NavigationState navigation_state; + char* feed_back_str; + Pose2D current_pose; + bool goal_checked; + bool is_ready; +} NavFeedback; + +// ============================================================================ +// String Management +// ============================================================================ + +/** + * @brief Free a string allocated by the library + * @param str String to free + */ +void nav_c_api_free_string(char* str); + +// ============================================================================ +// State Conversion +// ============================================================================ + +/** + * @brief Convert a State enum to its string representation + * @param state Enum value of NavigationState + * @return String representation (caller must free with nav_c_api_free_string) + */ +char* navigation_state_to_string(NavigationState state); + +// ============================================================================ +// Helper Functions +// ============================================================================ + +/** + * @brief Creates a target pose by offsetting a given 2D pose along its heading direction + * @param pose_x X coordinate of the original pose + * @param pose_y Y coordinate of the original pose + * @param pose_theta Heading angle in radians + * @param frame_id The coordinate frame ID (null-terminated string) + * @param offset_distance Distance to offset along heading (positive = forward, negative = backward) + * @param out_goal Output parameter for the offset pose + * @return true on success, false on failure + */ +bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta, + const char* frame_id, double offset_distance, + PoseStamped* out_goal); + +/** + * @brief Creates an offset target pose from a given PoseStamped + * @param in_pose Input pose + * @param offset_distance Distance to offset along heading direction + * @param out_goal Output parameter for the offset pose + * @return true on success, false on failure + */ +bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance, + PoseStamped* out_goal); + +// ============================================================================ +// Navigation Handle Management +// ============================================================================ + +/** + * @brief Create a new navigation instance + * @return Navigation handle, or NULL on failure + */ +NavigationHandle navigation_create(void); + +/** + * @brief Destroy a navigation instance + * @param handle Navigation handle to destroy + */ +void navigation_destroy(NavigationHandle handle); + +// ============================================================================ +// TF Listener Management +// ============================================================================ + +/** + * @brief Create a TF listener instance + * @return TF listener handle, or NULL on failure + */ +TFListenerHandle tf_listener_create(void); + +/** + * @brief Destroy a TF listener instance + * @param handle TF listener handle to destroy + */ +void tf_listener_destroy(TFListenerHandle handle); + +// ============================================================================ +// Navigation Interface Methods +// ============================================================================ + +/** + * @brief Initialize the navigation system + * @param handle Navigation handle + * @param tf_handle TF listener handle + * @return true on success, false on failure + */ +bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle); + +/** + * @brief Set the robot's footprint (outline shape) + * @param handle Navigation handle + * @param points Array of points representing the footprint polygon + * @param point_count Number of points in the array + * @return true on success, false on failure + */ +bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count); + +/** + * @brief Send a goal for the robot to navigate to + * @param handle Navigation handle + * @param goal Target pose in the global frame + * @param xy_goal_tolerance Acceptable error in X/Y (meters) + * @param yaw_goal_tolerance Acceptable angular error (radians) + * @return true if goal was accepted and sent successfully + */ +bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal, + double xy_goal_tolerance, double yaw_goal_tolerance); + +/** + * @brief Send a docking goal to a predefined marker + * @param handle Navigation handle + * @param marker Marker name or ID (null-terminated string) + * @param goal Target pose for docking + * @param xy_goal_tolerance Acceptable XY error (meters) + * @param yaw_goal_tolerance Acceptable heading error (radians) + * @return true if docking command succeeded + */ +bool navigation_dock_to(NavigationHandle handle, const char* marker, + const PoseStamped* goal, + double xy_goal_tolerance, double yaw_goal_tolerance); + +/** + * @brief Move straight toward the target position + * @param handle Navigation handle + * @param goal Target pose + * @param xy_goal_tolerance Acceptable positional error (meters) + * @return true if command issued successfully + */ +bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal, + double xy_goal_tolerance); + +/** + * @brief Rotate in place to align with target orientation + * @param handle Navigation handle + * @param goal Pose containing desired heading (only Z-axis used) + * @param yaw_goal_tolerance Acceptable angular error (radians) + * @return true if rotation command was sent successfully + */ +bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal, + double yaw_goal_tolerance); + +/** + * @brief Pause the robot's movement + * @param handle Navigation handle + */ +void navigation_pause(NavigationHandle handle); + +/** + * @brief Resume motion after a pause + * @param handle Navigation handle + */ +void navigation_resume(NavigationHandle handle); + +/** + * @brief Cancel the current goal and stop the robot + * @param handle Navigation handle + */ +void navigation_cancel(NavigationHandle handle); + +/** + * @brief Send limited linear velocity command + * @param handle Navigation handle + * @param linear_x Linear velocity in X direction + * @param linear_y Linear velocity in Y direction + * @param linear_z Linear velocity in Z direction + * @return true if the command was accepted + */ +bool navigation_set_twist_linear(NavigationHandle handle, + double linear_x, double linear_y, double linear_z); + +/** + * @brief Send limited angular velocity command + * @param handle Navigation handle + * @param angular_x Angular velocity around X axis + * @param angular_y Angular velocity around Y axis + * @param angular_z Angular velocity around Z axis + * @return true if the command was accepted + */ +bool navigation_set_twist_angular(NavigationHandle handle, + double angular_x, double angular_y, double angular_z); + +/** + * @brief Get the robot's pose as a PoseStamped + * @param handle Navigation handle + * @param out_pose Output parameter with the robot's current pose + * @return true if pose was successfully retrieved + */ +bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose); + +/** + * @brief Get the robot's pose as a 2D pose + * @param handle Navigation handle + * @param out_pose Output parameter with the robot's current 2D pose + * @return true if pose was successfully retrieved + */ +bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose); + +/** + * @brief Get navigation feedback + * @param handle Navigation handle + * @param out_feedback Output parameter with navigation feedback + * @return true if feedback was successfully retrieved + * @note The feed_back_str field must be freed using nav_c_api_free_string + */ +bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback); + +/** + * @brief Free navigation feedback structure + * @param feedback Feedback structure to free + */ +void navigation_free_feedback(NavFeedback* feedback); + +#ifdef __cplusplus +} +#endif + +#endif // NAVIGATION_C_API_H + diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp new file mode 100644 index 0000000..923bdc3 --- /dev/null +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -0,0 +1,432 @@ +#include "nav_c_api.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// ============================================================================ +// Internal Helper Functions +// ============================================================================ + +namespace { + // Convert C PoseStamped to C++ PoseStamped + geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) { + geometry_msgs::PoseStamped cpp_pose; + cpp_pose.header.seq = c_pose->header.seq; + cpp_pose.header.stamp.sec = c_pose->header.sec; + cpp_pose.header.stamp.nsec = c_pose->header.nsec; + if (c_pose->header.frame_id) { + cpp_pose.header.frame_id = c_pose->header.frame_id; + } + cpp_pose.pose.position.x = c_pose->pose.position.x; + cpp_pose.pose.position.y = c_pose->pose.position.y; + cpp_pose.pose.position.z = c_pose->pose.position.z; + cpp_pose.pose.orientation.x = c_pose->pose.orientation.x; + cpp_pose.pose.orientation.y = c_pose->pose.orientation.y; + cpp_pose.pose.orientation.z = c_pose->pose.orientation.z; + cpp_pose.pose.orientation.w = c_pose->pose.orientation.w; + return cpp_pose; + } + + // Convert C++ PoseStamped to C PoseStamped + void cpp_to_c_pose_stamped(const geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) { + c_pose->header.seq = cpp_pose.header.seq; + c_pose->header.sec = cpp_pose.header.stamp.sec; + c_pose->header.nsec = cpp_pose.header.stamp.nsec; + if (!cpp_pose.header.frame_id.empty()) { + c_pose->header.frame_id = strdup(cpp_pose.header.frame_id.c_str()); + } else { + c_pose->header.frame_id = nullptr; + } + c_pose->pose.position.x = cpp_pose.pose.position.x; + c_pose->pose.position.y = cpp_pose.pose.position.y; + c_pose->pose.position.z = cpp_pose.pose.position.z; + c_pose->pose.orientation.x = cpp_pose.pose.orientation.x; + c_pose->pose.orientation.y = cpp_pose.pose.orientation.y; + c_pose->pose.orientation.z = cpp_pose.pose.orientation.z; + c_pose->pose.orientation.w = cpp_pose.pose.orientation.w; + } + + // Convert C++ Pose2D to C Pose2D + void cpp_to_c_pose_2d(const geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) { + c_pose->x = cpp_pose.x; + c_pose->y = cpp_pose.y; + c_pose->theta = cpp_pose.theta; + } + + // Convert C++ NavFeedback to C NavFeedback + void cpp_to_c_nav_feedback(const move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) { + c_feedback->navigation_state = static_cast(static_cast(cpp_feedback.navigation_state)); + if (!cpp_feedback.feed_back_str.empty()) { + c_feedback->feed_back_str = strdup(cpp_feedback.feed_back_str.c_str()); + } else { + c_feedback->feed_back_str = nullptr; + } + c_feedback->current_pose.x = cpp_feedback.current_pose.x; + c_feedback->current_pose.y = cpp_feedback.current_pose.y; + c_feedback->current_pose.theta = cpp_feedback.current_pose.theta; + c_feedback->goal_checked = cpp_feedback.goal_checked; + c_feedback->is_ready = cpp_feedback.is_ready; + } +} + +// ============================================================================ +// String Management +// ============================================================================ + +extern "C" void nav_c_api_free_string(char* str) { + if (str) { + free(str); + } +} + +// ============================================================================ +// State Conversion +// ============================================================================ + +extern "C" char* navigation_state_to_string(NavigationState state) { + move_base_core::State cpp_state = static_cast(static_cast(state)); + std::string str = move_base_core::toString(cpp_state); + return strdup(str.c_str()); +} + +// ============================================================================ +// Helper Functions +// ============================================================================ + +extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double pose_theta, + const char* frame_id, double offset_distance, + PoseStamped* out_goal) { + if (!out_goal || !frame_id) { + return false; + } + + geometry_msgs::Pose2D pose; + pose.x = pose_x; + pose.y = pose_y; + pose.theta = pose_theta; + + geometry_msgs::PoseStamped result = move_base_core::offset_goal(pose, std::string(frame_id), offset_distance); + cpp_to_c_pose_stamped(result, out_goal); + return true; +} + +extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, double offset_distance, + PoseStamped* out_goal) { + if (!in_pose || !out_goal) { + return false; + } + + geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose); + geometry_msgs::PoseStamped result = move_base_core::offset_goal(cpp_pose, offset_distance); + cpp_to_c_pose_stamped(result, out_goal); + return true; +} + +// ============================================================================ +// Navigation Handle Management +// ============================================================================ + +extern "C" NavigationHandle navigation_create(void) { + try { + // Create a concrete MoveBase instance + // Using default constructor - initialization will be done via navigation_initialize() + move_base::MoveBase* move_base = new move_base::MoveBase(); + return static_cast(move_base); + } catch (const std::exception& e) { + // Log error if possible (in production, use proper logging) + return nullptr; + } catch (...) { + return nullptr; + } +} + +extern "C" void navigation_destroy(NavigationHandle handle) { + if (handle) { + move_base_core::BaseNavigation* nav = static_cast(handle); + delete nav; + } +} + +// ============================================================================ +// TF Listener Management +// ============================================================================ + +extern "C" TFListenerHandle tf_listener_create(void) { + try { + auto tf = std::make_shared(); + return new std::shared_ptr(tf); + } catch (...) { + return nullptr; + } +} + +extern "C" void tf_listener_destroy(TFListenerHandle handle) { + if (handle) { + auto* tf_ptr = static_cast*>(handle); + delete tf_ptr; + } +} + +// ============================================================================ +// Navigation Interface Methods +// ============================================================================ + +extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) +{ + printf("[%s:%d] Begin: navigation_initialize\n", __FILE__, __LINE__); + if (!handle || !tf_handle) { + printf("[%s:%d] Error: Invalid parameters\n", __FILE__, __LINE__); + return false; + } + try { + printf("[%s:%d] Initialize navigation\n", __FILE__, __LINE__); + move_base_core::BaseNavigation* nav = static_cast(handle); + auto* tf_ptr = static_cast*>(tf_handle); + + auto create_plugin = boost::dll::import_alias( + "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Packages/move_base/libmove_base.so", + "MoveBase", + boost::dll::load_mode::append_decorations); + + move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin(); + printf("[%s:%d] Navigation created\n", __FILE__, __LINE__); + if (nav_ptr == nullptr) { + printf("[%s:%d] Error: Failed to create navigation\n", __FILE__, __LINE__); + return false; + } + nav_ptr->initialize(*tf_ptr); + + return true; + } catch (const std::exception &e) { + printf("[%s:%d] Error: Failed to initialize navigation: %s\n", __FILE__, __LINE__, e.what()); + return false; + } catch (...) { + printf("[%s:%d] Error: Failed to initialize navigation\n", __FILE__, __LINE__); + return false; + } +} + +extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count) { + if (!handle || !points || point_count == 0) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + std::vector footprint; + footprint.reserve(point_count); + for (size_t i = 0; i < point_count; ++i) { + geometry_msgs::Point pt; + pt.x = points[i].x; + pt.y = points[i].y; + pt.z = points[i].z; + footprint.push_back(pt); + } + nav->setRobotFootprint(footprint); + return true; + } catch (...) { + return false; + } +} + +extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal, + double xy_goal_tolerance, double yaw_goal_tolerance) { + if (!handle || !goal) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); + return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); + } catch (...) { + return false; + } +} + +extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker, + const PoseStamped* goal, + double xy_goal_tolerance, double yaw_goal_tolerance) { + if (!handle || !marker || !goal) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); + return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); + } catch (...) { + return false; + } +} + +extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseStamped* goal, + double xy_goal_tolerance) { + if (!handle || !goal) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); + return nav->moveStraightTo(cpp_goal, xy_goal_tolerance); + } catch (...) { + return false; + } +} + +extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* goal, + double yaw_goal_tolerance) { + if (!handle || !goal) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); + return nav->rotateTo(cpp_goal, yaw_goal_tolerance); + } catch (...) { + return false; + } +} + +extern "C" void navigation_pause(NavigationHandle handle) { + if (handle) { + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + nav->pause(); + } catch (...) { + // Ignore exceptions + } + } +} + +extern "C" void navigation_resume(NavigationHandle handle) { + if (handle) { + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + nav->resume(); + } catch (...) { + // Ignore exceptions + } + } +} + +extern "C" void navigation_cancel(NavigationHandle handle) { + if (handle) { + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + nav->cancel(); + } catch (...) { + // Ignore exceptions + } + } +} + +extern "C" bool navigation_set_twist_linear(NavigationHandle handle, + double linear_x, double linear_y, double linear_z) { + if (!handle) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + geometry_msgs::Vector3 linear; + linear.x = linear_x; + linear.y = linear_y; + linear.z = linear_z; + return nav->setTwistLinear(linear); + } catch (...) { + return false; + } +} + +extern "C" bool navigation_set_twist_angular(NavigationHandle handle, + double angular_x, double angular_y, double angular_z) { + if (!handle) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + geometry_msgs::Vector3 angular; + angular.x = angular_x; + angular.y = angular_y; + angular.z = angular_z; + return nav->setTwistAngular(angular); + } catch (...) { + return false; + } +} + +extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out_pose) { + if (!handle || !out_pose) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + geometry_msgs::PoseStamped cpp_pose; + if (nav->getRobotPose(cpp_pose)) { + cpp_to_c_pose_stamped(cpp_pose, out_pose); + return true; + } + return false; + } catch (...) { + return false; + } +} + +extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose) { + if (!handle || !out_pose) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + geometry_msgs::Pose2D cpp_pose; + if (nav->getRobotPose(cpp_pose)) { + cpp_to_c_pose_2d(cpp_pose, out_pose); + return true; + } + return false; + } catch (...) { + return false; + } +} + +extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) { + if (!handle || !out_feedback) { + return false; + } + + try { + move_base_core::BaseNavigation* nav = static_cast(handle); + if (nav->nav_feedback_) { + cpp_to_c_nav_feedback(*nav->nav_feedback_, out_feedback); + return true; + } + return false; + } catch (...) { + return false; + } +} + +extern "C" void navigation_free_feedback(NavFeedback* feedback) { + if (feedback) { + nav_c_api_free_string(feedback->feed_back_str); + feedback->feed_back_str = nullptr; + } +} + diff --git a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt new file mode 100644 index 0000000..a87d9d5 --- /dev/null +++ b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.10) + +# Tên dự án +project(score_algorithm VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +set(PACKAGES_DIR nav_2d_msgs nav_2d_utils nav_core nav_core2 geometry_msgs mkt_msgs angles) + +find_package(Boost REQUIRED COMPONENTS filesystem system) + +# Thư mục include +include_directories( + ${PROJECT_SOURCE_DIR}/include +) +# Tạo thư viện shared (.so) + +add_library(score_algorithm src/score_algorithm.cpp) +target_link_libraries(score_algorithm + PUBLIC + ${PACKAGES_DIR} + PRIVATE + Boost::filesystem + Boost::system +) +target_include_directories(score_algorithm PUBLIC + $ + $) + +# Cài đặt header files +install(DIRECTORY include/score_algorithm + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +# Cài đặt library +install(TARGETS score_algorithm + EXPORT score_algorithm-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +# Export targets +install(EXPORT score_algorithm-targets + FILE score_algorithm-targets.cmake + DESTINATION lib/cmake/score_algorithm) + +# Tùy chọn build +option(BUILD_SHARED_LIBS "Build shared libraries" ON) +option(BUILD_TESTS "Build test programs" OFF) + +# Flags biên dịch +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Headers found:") +foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") +endforeach() +message(STATUS "=================================") \ No newline at end of file diff --git a/src/Algorithms/Cores/score_algorithm/include/angles/angles.h b/src/Algorithms/Cores/score_algorithm/include/angles/angles.h new file mode 100644 index 0000000..2b8c866 --- /dev/null +++ b/src/Algorithms/Cores/score_algorithm/include/angles/angles.h @@ -0,0 +1,26 @@ +#ifndef ANGLES_ANGLES_H +#define ANGLES_ANGLES_H + +#include + +namespace angles +{ + +/** + * @brief Normalize an angle to the range [-π, π] + * @param angle The angle in radians to normalize + * @return The normalized angle in the range [-π, π] + */ +inline double normalize_angle(double angle) +{ + while (angle > M_PI) + angle -= 2.0 * M_PI; + while (angle < -M_PI) + angle += 2.0 * M_PI; + return angle; +} + +} // namespace angles + +#endif // ANGLES_ANGLES_H + diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h new file mode 100755 index 0000000..2f97b48 --- /dev/null +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h @@ -0,0 +1,56 @@ +#ifndef _SCORE_GOAL_CHECKER_H_INCLUDED__ +#define _SCORE_GOAL_CHECKER_H_INCLUDED__ + +#include +#include +#include +#include +#include +#include +#include + +namespace score_algorithm +{ + + /** + * @class GoalChecker + * @brief Function-object for checking whether a goal has been reached + * + * This class defines the plugin interface for determining whether you have reached + * the goal state. This primarily consists of checking the relative positions of two poses + * (which are presumed to be in the same frame). It can also check the velocity, as some + * applications require that robot be stopped to be considered as having reached the goal. + */ + class GoalChecker + { + public: + using Ptr = std::shared_ptr; + + virtual ~GoalChecker() {} + + /** + * @brief Initialize any parameters from the NodeHandle + * @param nh NodeHandle for grabbing parameters + */ + virtual void initialize(const ros::NodeHandle& nh) = 0; + + /** + * @brief Reset the state of the goal checker (if any) + */ + virtual void reset() {} + + /** + * @brief Check whether the goal should be considered reached + * @param query_pose The pose to check + * @param goal_pose The pose to check against + * + * @param velocity The robot's current velocity + * @return True if goal is reached + */ + virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped& query_pose, const nav_2d_msgs::Pose2DStamped& goal_pose, + const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D& velocity) = 0; + }; + +} // namespace score_algorithm + +#endif // _SCORE_GOAL_CHECKER_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h new file mode 100755 index 0000000..76ed97f --- /dev/null +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h @@ -0,0 +1,169 @@ +#ifndef _SCORE_ALGORITHM_H_INCLUDED__ +#define _SCORE_ALGORITHM_H_INCLUDED__ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#define FORWARD 1 +#define BACKWARD -1 + +namespace score_algorithm +{ + class ScoreAlgorithm + { + public: + using Ptr = boost::shared_ptr; + + inline double smoothstep(double x) + { + x = std::clamp(x, 0.0, 1.0); + return x * x * (3 - 2 * x); + } + + /** + * @brief Initialize parameters as needed + * @param name Namespace for this planner + * @param tf TFListener pointer + * @param costmap_ros Costmap pointer + */ + virtual void initialize(YAML::Node &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_ros, + const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0; + + /** + * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories + * + * Subclasses may overwrite. Return false in case there is any error. + * + * @param pose Current pose (costmap frame) + * @param velocity Current velocity + * @param goal The final goal (costmap frame) + * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points + */ + virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, + const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, + double &x_direction, double &y_direction, double &theta_direction) = 0; + + /** + * @brief Calculating algorithm + * @param pose + * @param velocity + * @param traj + */ + virtual mkt_msgs::Trajectory2D calculator( + const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0; + + /** + * @brief Reset all data + */ + virtual void reset() {}; + + /** + * @brief Stoping move navigation + */ + virtual void stop() {}; + + /** + * @brief resume move navigation after stopped + */ + virtual void resume() {}; + + std::string getName() + { + return name_; + } + + protected: + double sign(double x) + { + return x < 0.0 ? -1.0 : 1.0; + } + + /** + * @brief Get goal index + * @param robot_pose + * @param plan + * @param distance + * @param start_index + * @param last_valid_index + * @return goal index + */ + virtual unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector &plan, + const double distance, unsigned int start_index, unsigned int last_valid_index) const; + + /** + * @brief Get goal pose + * @param robot_pose + * @param velocity + * @param global_plan + * @param sub_goal + * @param S + * @return true if goal pose is found, false otherwise + */ + virtual bool computePlanCommand(const nav_2d_msgs::Pose2DStamped &robot_pose, const nav_2d_msgs::Twist2D &velocity, const double &S, + const nav_2d_msgs::Path2D &global_plan, unsigned int &start_index_global_plan, unsigned int &goal_index_global_plan, nav_2d_msgs::Path2D &result); + + /** + * @brief Find sub goal index + * @param plan + * @param index_s + * @return true if sub goal index is found, false otherwise + */ + virtual bool findSubGoalIndex(const std::vector &plan, std::vector &seq_s, std::vector &mutes); + + /** + * @brief Calculate journey + * @param plan + * @param start_index + * @param last_valid_index + * @return journey + */ + double journey(const std::vector &plan, const unsigned int start_index, const unsigned int last_valid_index) const; + + /** + * @brief Check whether the robot is stopped or not + * @param velocity The current odometry information for the robot + * @param rot_stopped_velocity The rotational velocity below which the robot is considered stopped + * @param trans_stopped_velocity The translational velocity below which the robot is considered stopped + * @return True if the robot is stopped, false otherwise + */ + bool stopped(const nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity); + + void clear(); + + std::string name_; + TFListenerPtr tf_; + costmap_2d::Costmap2DROBOT *costmap_robot_; + TrajectoryGenerator::Ptr traj_; + + int index_samples_; + bool follow_step_path_; + double xy_local_goal_tolerance_; + double old_xy_goal_tolerance_; + double angle_threshold_; + + bool enable_publish_; + // ros::Publisher reached_intermediate_goals_pub_; + // ros::Publisher current_goal_pub_; + // ros::Publisher closet_robot_goal_pub_; + // ros::Publisher transformed_plan_pub_, compute_plan_pub_; + + std::vector reached_intermediate_goals_; + std::vector start_index_saved_vt_; + unsigned int sub_goal_index_saved_, sub_goal_seq_saved_; + unsigned int sub_start_index_saved_, sub_start_seq_saved_; + }; +} // namespace score_algorithm + +#endif // SCORE_ALGORITHM_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h new file mode 100755 index 0000000..a88a962 --- /dev/null +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h @@ -0,0 +1,136 @@ +#ifndef _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H +#define _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace score_algorithm +{ + + /** + * @class TrajectoryGenerator + * @brief Interface for iterating through possible velocities and creating trajectories + * + * This class defines the plugin interface for two separate but related components. + * + * First, this class provides an iterator interface for exploring all of the velocities + * to search, given the current velocity. + * + * Second, the class gives an independent interface for creating a trajectory from a twist, + * i.e. projecting it out in time and space. + * + * Both components rely heavily on the robot's kinematic model, and can share many parameters, + * which is why they are grouped into a singular class. + */ + class TrajectoryGenerator + { + public: + using Ptr = boost::shared_ptr; + + virtual ~TrajectoryGenerator() {} + + /** + * @brief Initialize parameters as needed + * @param nh NodeHandle to read parameters from + */ + virtual void initialize(const YAML::Node &nh) = 0; + + /** + * @brief Reset the state (if any) when the planner gets a new goal + */ + virtual void reset() {} + + /** + * @brief Start a new iteration based on the current velocity + * @param current_velocity + */ + virtual void startNewIteration(const nav_2d_msgs::Twist2D ¤t_velocity) = 0; + + /** + * @brief Set direct of robot based on the current velocity + * @param direct + */ + virtual void setDirect(int *xytheta_direct) = 0; + + /** + * @brief set velocity for x, y asix of the robot + * @param linear the velocity command + * @return True if set is done, false otherwise + */ + virtual bool setTwistLinear(geometry_msgs::Vector3 linear) = 0; + + /** + * @brief get velocity for x, y asix of the robot + * @param linear The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistLinear(bool direct) = 0; + + /** + * @brief Set velocity theta for z asix of the robot + * @param angular the command velocity + * @return True if set is done, false otherwise + */ + virtual bool setTwistAngular(geometry_msgs::Vector3 angular) = 0; + + /** + * @brief Get velocity theta for z asix of the robot + * @param direct The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistAngular(bool direct) = 0; + + /** + * @brief Test to see whether there are more twists to test + * @return True if more twists, false otherwise + */ + virtual bool hasMoreTwists() = 0; + + /** + * @brief Return the next twist and advance the iteration + * @return The Twist! + */ + virtual nav_2d_msgs::Twist2D nextTwist() = 0; + + /** + * @brief Get all the twists for an iteration. + * + * Note: Resets the iterator if one is in process + * + * @param current_velocity + * @return all the twists + */ + virtual std::vector getTwists(const nav_2d_msgs::Twist2D ¤t_velocity) + { + std::vector twists; + startNewIteration(current_velocity); + while (hasMoreTwists()) + { + twists.push_back(nextTwist()); + } + return twists; + } + + /** + * @brief Given a cmd_vel in the robot's frame and initial conditions, generate a Trajectory2D + * @param start_pose Current robot location + * @param start_vel Current robot velocity + * @param cmd_vel The desired command velocity + */ + virtual mkt_msgs::Trajectory2D generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose, + const nav_2d_msgs::Path2D &transformed_plan, + const nav_2d_msgs::Twist2D &start_vel, + const nav_2d_msgs::Twist2D &cmd_vel) = 0; + + virtual YAML::Node getNodeHandle() = 0; + }; + +} // namespace score_algorithm + +#endif // _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp new file mode 100644 index 0000000..25f5de3 --- /dev/null +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -0,0 +1,470 @@ +#include +#include +#include +#include + +unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector &plan, + const double distance, 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 + 1; + const double start_direction_x = plan[goal_index].pose.x - plan[start_index].pose.x; + const double start_direction_y = plan[goal_index].pose.y - plan[start_index].pose.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); + double S = sqrt(pow(start_direction_x, 2) + pow(start_direction_y, 2)); + // double angle_threshold = 0.0; + for (unsigned int i = start_index + 1; i <= last_valid_index; i++) + { + const double current_direction_x = plan[i].pose.x - plan[i - 1].pose.x; + const double current_direction_y = plan[i].pose.y - plan[i - 1].pose.y; + const double dd = sqrt(pow(current_direction_x, 2) + pow(current_direction_y, 2)); + S += dd; + if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + { + double current_angle = atan2(current_direction_y, current_direction_x); + goal_index = i; + if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= angle_threshold_ || S >= distance) + { + break; + } + } + } + } + if (last_valid_index - goal_index == 1) + goal_index = last_valid_index; + return goal_index; +} + +bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector &plan, std::vector &seq_s, std::vector &mutes) +{ + if (plan.empty()) + { + return false; + } + double x_direction_saved = 0.0; + geometry_msgs::PoseArray poseArrayMsg; + poseArrayMsg.header.stamp = robot::Time::now(); + poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID(); + + nav_2d_msgs::Pose2DStamped p1, p2, p3; + + if (plan.size() < 3) + { + seq_s.push_back(plan.back().header.seq); + mutes.push_back(plan.back()); + } + else + { + unsigned int i = 0; + p1 = plan[i]; + for (i = 0; i < (unsigned int)plan.size() - 2; i++) + { + double egde1_angle, egde2_angle; + p2 = plan[i + 1]; + if ((fabs(p2.pose.y - p1.pose.y) > 1e-9 || fabs(p2.pose.x - p1.pose.x) > 1e-9)) + { + egde1_angle = atan2(p2.pose.y - p1.pose.y, p2.pose.x - p1.pose.x); + } + else + continue; + + const double target_direction = cos(fabs(egde1_angle - p2.pose.theta)); + + if (target_direction * x_direction_saved < 0.0) + { + if (std::find(seq_s.begin(), seq_s.end(), plan[i].header.seq) == seq_s.end()) + { + seq_s.push_back(plan[i].header.seq); + mutes.push_back(plan[i]); + geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan[i]).pose; + poseArrayMsg.poses.push_back(pose); + } + } + x_direction_saved = target_direction; + + p3 = plan[i + 2]; + if ((fabs(p3.pose.y - p2.pose.y) > 1e-9 || fabs(p3.pose.x - p2.pose.x) > 1e-9)) + { + egde2_angle = atan2(p3.pose.y - p2.pose.y, p3.pose.x - p2.pose.x); + p1 = p2; + } + else + continue; + + double thresh_angle = 0.3; + double angle = fabs(remainder(egde1_angle - egde2_angle, 2 * M_PI)); + if (angle >= thresh_angle || angle >= M_PI - thresh_angle) + { + if (std::find(seq_s.begin(), seq_s.end(), plan[i].header.seq) == seq_s.end()) + { + seq_s.push_back(plan[i + 1].header.seq); + mutes.push_back(plan[i + 1]); + geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan[i + 1]).pose; + poseArrayMsg.poses.push_back(pose); + } + p2 = p3; + } + } + } + + if (seq_s.empty()) + { + seq_s.push_back(plan.back().header.seq); + mutes.push_back(plan.back()); + geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan.back()).pose; + poseArrayMsg.poses.push_back(pose); + } + else + { + if (seq_s.back() < plan.back().header.seq) + { + seq_s.push_back(plan.back().header.seq); + mutes.push_back(plan.back()); + geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan.back()).pose; + poseArrayMsg.poses.push_back(pose); + } + } + return true; +} + +bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose2DStamped &robot_pose, const nav_2d_msgs::Twist2D &velocity, const double &S, + const nav_2d_msgs::Path2D &global_plan, unsigned int &start_index_global_plan, unsigned int &goal_index_global_plan, nav_2d_msgs::Path2D &result) +{ + result.poses.clear(); + if (global_plan.poses.empty()) + { + std::cerr << "ScoreAlgorithm: The global plan was empty." << std::endl; + return false; + } + + std::vector seq_s; + std::vector mutes; + if (!this->findSubGoalIndex(global_plan.poses, seq_s, mutes)) + { + std::cerr << "ScoreAlgorithm: Cannot find SubGoal Index" << std::endl; + return false; + } + + unsigned int sub_goal_index; + unsigned int sub_start_index; + + bool found_sub_goal_index_saved = false, found_sub_start_index_saved = false; + for (unsigned int i = 0; i < (unsigned int)global_plan.poses.size(); ++i) + { + if (global_plan.poses[i].header.seq == sub_goal_seq_saved_) + { + sub_goal_index_saved_ = i; + found_sub_goal_index_saved = true; + } + if (global_plan.poses[i].header.seq == sub_start_seq_saved_) + { + sub_start_index_saved_ = i; + found_sub_start_index_saved = true; + } + if (found_sub_goal_index_saved && found_sub_start_index_saved) + { + break; + } + } + + sub_goal_index = !found_sub_goal_index_saved ? (unsigned int)global_plan.poses.size() : sub_goal_index_saved_; + sub_start_index = !found_sub_start_index_saved ? 0 : sub_start_index_saved_; + + std::vector index_s; + for (auto seq : seq_s) + { + for (unsigned int i = 0; i < (unsigned int)global_plan.poses.size(); ++i) + { + if (global_plan.poses[i].header.seq == seq) + { + index_s.push_back(i); + break; + } + } + } + + // Handle empty seq_s + if (index_s.empty()) + { + sub_goal_index = (unsigned int)global_plan.poses.size(); + sub_goal_seq_saved_ = global_plan.poses.back().header.seq; + std::cout << "ScoreAlgorithm: seq_s is empty, setting sub_goal_index to " << sub_goal_index << std::endl; + } + else + { + // Update sub_goal_index if index_s.front() is greater and valid + if (index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size()) + { + sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front(); + sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; + std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl; + } + + // Process index_s with multiple elements + if (index_s.size() > 1) + { + for (size_t i = 0; i < index_s.size(); ++i) + { + if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size()) + { + std::cout << "ScoreAlgorithm: Invalid index index_s[" << i - 1 << "]=" << index_s[i - 1] << " or index_s[" << i << "]=" << index_s[i] << ", plan size=" << (unsigned int)global_plan.poses.size() << std::endl; + continue; + } + + double dx = robot_pose.pose.x - global_plan.poses[index_s[i - 1]].pose.x; + double dy = robot_pose.pose.y - global_plan.poses[index_s[i - 1]].pose.y; + double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[index_s[i - 1]].pose.theta); + double x = cos(global_plan.poses[index_s[i - 1]].pose.theta) * dx + sin(global_plan.poses[index_s[i - 1]].pose.theta) * dy; + double y = -sin(global_plan.poses[index_s[i - 1]].pose.theta) * dx + cos(global_plan.poses[index_s[i - 1]].pose.theta) * dy; + + if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1) + { + double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; + if (fabs(tolerance) <= xy_local_goal_tolerance_) + { + // ROS_INFO_THROTTLE(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[index_s[i - 1]].pose.theta); + // ROS_INFO_THROTTLE(0.1, "tolerance 1 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); + if (index_s[i] > sub_goal_index_saved_) + { + // ROS_INFO("%d %d %d %d %d", index_s[i], sub_goal_index, sub_goal_index_saved_, sub_goal_seq_saved_, (unsigned int)global_plan.poses.size()); + // ROS_INFO_NAMED("ScoreAlgorithm", "Following from %u to %d", sub_goal_index, i < (unsigned int)(index_s.size() - 1) ? index_s[i] + 1 : (unsigned int)global_plan.poses.size()); + sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1; + sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; + old_xy_goal_tolerance_ = 0.0; + if (follow_step_path_) + { + sub_start_index = index_s[i - 1]; + sub_start_seq_saved_ = global_plan.poses[sub_start_index].header.seq; + } + break; + } + } + } + else + old_xy_goal_tolerance_ = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; + } + } + else + { + // Validate sub_goal_index + if (sub_goal_index >= (unsigned int)global_plan.poses.size() - 1) + { + sub_goal_index = (unsigned int)global_plan.poses.size() - 1; + sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; + std::cout << "ScoreAlgorithm: Invalid sub_goal_index, setting to " << sub_goal_index << std::endl; + } + else + { + double dx = robot_pose.pose.x - global_plan.poses[sub_goal_index].pose.x; + double dy = robot_pose.pose.y - global_plan.poses[sub_goal_index].pose.y; + double theta = angles::normalize_angle(robot_pose.pose.theta - global_plan.poses[sub_goal_index].pose.theta); + double x = cos(global_plan.poses[sub_goal_index].pose.theta) * dx + sin(global_plan.poses[sub_goal_index].pose.theta) * dy; + double y = -sin(global_plan.poses[sub_goal_index].pose.theta) * dx + cos(global_plan.poses[sub_goal_index].pose.theta) * dy; + if (std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_local_goal_tolerance_ + 0.1) + { + double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; + if (fabs(tolerance) <= xy_local_goal_tolerance_) + { + // ROS_INFO_THROTTLE(0.1, "%f %f %f %f ", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[sub_goal_index].pose.theta); + // ROS_INFO_THROTTLE(0.1, "tolerance 2 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); + sub_goal_index = (unsigned int)global_plan.poses.size() - 1; + sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; + old_xy_goal_tolerance_ = 0.0; + std::cout << "ScoreAlgorithm: Single sub-goal reached, setting sub_goal_index to " << sub_goal_index << std::endl; + } + } + else + old_xy_goal_tolerance_ = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; + } + } + } + if (sub_start_index >= sub_goal_index) + { + std::cerr << "ScoreAlgorithm: Sub path is empty" << std::endl; + return false; + } + + // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map + // Tìm điểm pose bắt đầu trên kế hoạch đường đi gần nhất với robot đồng thời có tồn tại trong local map + unsigned int start_index = 0; // số vị trí pose trong kế hoạch đường đi + unsigned int closet_index = 0; + double distance_to_start = std::numeric_limits::infinity(); // Xét giá trị là vô cùng oo + bool started_path = false; + for (unsigned int i = sub_start_index; i < sub_goal_index; ++i) + { + // Still on the costmap. Continue. + double distance = fabs(nav_2d_utils::poseDistance(global_plan.poses[i].pose, robot_pose.pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét + if (distance_to_start > distance) + { + start_index = i; + closet_index = start_index; + distance_to_start = distance; + started_path = true; + } + } + + // Nếu khồng tìm được điểm gần với robot nhất thì trả về false + if (!started_path) + { + std::cerr << "ScoreAlgorithm: None of the points of the global plan were in the local costmap." << std::endl; + return false; + } + std::stringstream ss; + start_index_saved_vt_.push_back(global_plan.poses[start_index].header.seq); + if (start_index_saved_vt_.size() > index_samples_) + start_index_saved_vt_.erase(start_index_saved_vt_.begin()); + + unsigned num = 0; + if (!start_index_saved_vt_.empty()) + { + for (int i = 0; i < (int)start_index_saved_vt_.size() - 1; i++) + { + if (start_index_saved_vt_[i] == start_index_saved_vt_[i + 1]) + num++; + ss << start_index_saved_vt_[i] << " "; + } + } + if (num < index_samples_ - 1 && !start_index_saved_vt_.empty()) + { + auto max_iter = *(std::max_element(start_index_saved_vt_.begin(), start_index_saved_vt_.end())); + unsigned int max_start_index = start_index; + for (unsigned int i = 0; i < (unsigned int)global_plan.poses.size(); i++) + { + if (global_plan.poses[i].header.seq == max_iter) + { + max_start_index = i; + break; + } + } + double S1 = journey(global_plan.poses, start_index, (unsigned int)global_plan.poses.size() - 1); + double S2 = journey(global_plan.poses, max_start_index, (unsigned int)global_plan.poses.size() - 1); + + if (S2 - S1) + { + start_index = max_start_index; + } + } + // ROS_INFO_STREAM_THROTTLE(0.1, ss.str() << "\n"); + + // find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map + // Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map + if (start_index < 1) + start_index = 1; + // unsigned int last_valid_index = (unsigned int)global_plan.poses.size() - 1; + unsigned int last_valid_index = sub_goal_index; + + // 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. + // Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan + unsigned int goal_index = start_index; + while (goal_index < last_valid_index) + { + // Tìm kiếm vị trí điểm mục tiêu phù hợp + goal_index = getGoalIndex(robot_pose.pose, global_plan.poses, S, start_index, last_valid_index); + + // check if goal already reached + bool goal_already_reached = false; + + // geometry_msgs::PoseArray poseArrayMsg; + // poseArrayMsg.header.stamp = ros::Time::now(); + // poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID(); + // int c = 0; + for (auto &reached_intermediate_goal : reached_intermediate_goals_) + { + double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose)); + + // geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose); + // poseArrayMsg.poses.push_back(pose); + + 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.pose, global_plan.poses[start_index].pose); + if (distance >= xy_local_goal_tolerance_) + break; + } + break; + } + } + + if (!goal_already_reached) + { + // new goal not in reached_intermediate_goals_ + double distance = fabs(nav_2d_utils::poseDistance(global_plan.poses[start_index].pose, global_plan.poses[goal_index].pose)); + if (distance < xy_local_goal_tolerance_) + { + // the robot has currently reached the goal + reached_intermediate_goals_.push_back(global_plan.poses[goal_index]); + std::cout << "ScoreAlgorithm: Number of reached_intermediate goals: " << reached_intermediate_goals_.size() << std::endl; + } + else + { + // we've found a new goal! + break; + } + } + } + + if (start_index > goal_index) + start_index = goal_index; + if (goal_index > last_valid_index) + goal_index = last_valid_index; + if ((unsigned int)global_plan.poses.size() == 2 && !global_plan.poses.empty()) + { + start_index = 0; + goal_index = 1; + } + + if (goal_index == (unsigned int)global_plan.poses.size() - 1) + start_index = (unsigned int)global_plan.poses.size() - 2; + + start_index_global_plan = start_index; + goal_index_global_plan = goal_index; + + nav_2d_msgs::Pose2DStamped sub_pose; + sub_pose = global_plan.poses[closet_index]; + + nav_2d_msgs::Pose2DStamped sub_goal; + sub_goal = global_plan.poses[goal_index]; + + result.header = global_plan.header; + for (int i = closet_index; i <= goal_index; i++) + { + result.poses.push_back(global_plan.poses[i]); + } + return true; +} + +double score_algorithm::ScoreAlgorithm::journey(const std::vector &plan, const unsigned int start_index, const unsigned int last_valid_index) const +{ + double S = 0; + if (last_valid_index - start_index > 1) + { + for (int i = start_index; i < last_valid_index; i++) + { + S += nav_2d_utils::poseDistance(plan[i].pose, plan[i + 1].pose); + } + } + return S; +} + +bool score_algorithm::ScoreAlgorithm::stopped(const nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity) +{ + return fabs(velocity.theta) <= rot_stopped_velocity && fabs(velocity.x) <= trans_stopped_velocity && fabs(velocity.y) <= trans_stopped_velocity; +} + +void score_algorithm::ScoreAlgorithm::clear() +{ + reached_intermediate_goals_.clear(); + start_index_saved_vt_.clear(); + sub_goal_index_saved_ = sub_goal_seq_saved_ = 0; + sub_start_index_saved_, sub_start_seq_saved_ = 0; +} \ No newline at end of file diff --git a/src/Algorithms/Libraries/angles/CMakeLists.txt b/src/Algorithms/Libraries/angles/CMakeLists.txt new file mode 100644 index 0000000..a60896c --- /dev/null +++ b/src/Algorithms/Libraries/angles/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.10) +project(angles VERSION 1.0.0 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Create interface library (header-only) +add_library(angles INTERFACE) + +# Set include directories +target_include_directories(angles INTERFACE + $ + $ +) + +# Install headers +install(DIRECTORY include/angles + DESTINATION include + FILES_MATCHING PATTERN "*.h" +) + +# Install target +install(TARGETS angles + EXPORT angles-targets + INCLUDES DESTINATION include +) + +# Export targets +install(EXPORT angles-targets + FILE angles-targets.cmake + DESTINATION lib/cmake/angles +) + diff --git a/src/Algorithms/Libraries/angles/include/angles/angles.h b/src/Algorithms/Libraries/angles/include/angles/angles.h new file mode 100644 index 0000000..2b8c866 --- /dev/null +++ b/src/Algorithms/Libraries/angles/include/angles/angles.h @@ -0,0 +1,26 @@ +#ifndef ANGLES_ANGLES_H +#define ANGLES_ANGLES_H + +#include + +namespace angles +{ + +/** + * @brief Normalize an angle to the range [-π, π] + * @param angle The angle in radians to normalize + * @return The normalized angle in the range [-π, π] + */ +inline double normalize_angle(double angle) +{ + while (angle > M_PI) + angle -= 2.0 * M_PI; + while (angle < -M_PI) + angle += 2.0 * M_PI; + return angle; +} + +} // namespace angles + +#endif // ANGLES_ANGLES_H + diff --git a/src/Algorithms/Libraries/kalman/CMakeLists.txt b/src/Algorithms/Libraries/kalman/CMakeLists.txt new file mode 100755 index 0000000..6c3ea7d --- /dev/null +++ b/src/Algorithms/Libraries/kalman/CMakeLists.txt @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 3.10) +project(kalman VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# Find dependencies +find_package(Eigen3 REQUIRED) + +# Thư mục include +include_directories( + ${PROJECT_SOURCE_DIR}/include + ${EIGEN3_INCLUDE_DIRS} +) + +# Tìm tất cả file source +file(GLOB SOURCES "src/kalman.cpp") +file(GLOB HEADERS "include/kalman/kalman.h") + +# Tạo thư viện shared (.so) +add_library(kalman SHARED ${SOURCES} ${HEADERS}) + +# Link libraries +target_link_libraries(kalman + PUBLIC + Eigen3::Eigen +) + +# Set include directories +target_include_directories(kalman + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) + +# Tạo executable cho test +add_executable(kalman_node src/kalman-test.cpp) +target_link_libraries(kalman_node + PRIVATE + kalman + Eigen3::Eigen +) + +# Cài đặt header files +install(DIRECTORY include/kalman + DESTINATION include + FILES_MATCHING PATTERN "*.h" +) + +# Cài đặt library +install(TARGETS kalman + EXPORT kalman-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +# Cài đặt executable (optional) +install(TARGETS kalman_node + RUNTIME DESTINATION bin +) + +# Export targets +install(EXPORT kalman-targets + FILE kalman-targets.cmake + DESTINATION lib/cmake/kalman +) + +# Tùy chọn build +option(BUILD_SHARED_LIBS "Build shared libraries" ON) +option(BUILD_TESTS "Build test programs" OFF) + +# Flags biên dịch +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Eigen3 found: ${EIGEN3_FOUND}") +message(STATUS "Eigen3 include dir: ${EIGEN3_INCLUDE_DIRS}") +message(STATUS "=================================") diff --git a/src/Algorithms/Libraries/kalman/README.md b/src/Algorithms/Libraries/kalman/README.md new file mode 100755 index 0000000..17613be --- /dev/null +++ b/src/Algorithms/Libraries/kalman/README.md @@ -0,0 +1,18 @@ +Kalman Filter +============= +This is a basic Kalman filter implementation in C++ using the +[Eigen](http://eigen.tuxfamily.org/) library. It implements the algorithm +directly as found in [An Introduction to the Kalman Filter](http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf). + +There is a test program that estimates the motion of a projectile based on +noisy observations. To run it, use CMake: + + cd kalman-cpp + mkdir build + cd build + cmake .. + make + ./kalman-test + +Note: You may have to specify the path to your Eigen library in +`CMakeLists.txt`. diff --git a/src/Algorithms/Libraries/kalman/include/kalman/kalman.h b/src/Algorithms/Libraries/kalman/include/kalman/kalman.h new file mode 100755 index 0000000..7fa1525 --- /dev/null +++ b/src/Algorithms/Libraries/kalman/include/kalman/kalman.h @@ -0,0 +1,91 @@ +/** +* Kalman filter implementation using Eigen. Based on the following +* introductory paper: +* +* http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf +* +* @author: Hayk Martirosyan +* @date: 2014.11.15 +*/ + +#include + +#pragma once + +class KalmanFilter { + +public: + + /** + * Create a Kalman filter with the specified matrices. + * A - System dynamics matrix + * C - Output matrix + * Q - Process noise covariance + * R - Measurement noise covariance + * P - Estimate error covariance + */ + KalmanFilter( + double dt, + const Eigen::MatrixXd& A, + const Eigen::MatrixXd& C, + const Eigen::MatrixXd& Q, + const Eigen::MatrixXd& R, + const Eigen::MatrixXd& P + ); + + /** + * Create a blank estimator. + */ + KalmanFilter(); + + /** + * Initialize the filter with initial states as zero. + */ + void init(); + + /** + * Initialize the filter with a guess for initial states. + */ + void init(double t0, const Eigen::VectorXd& x0); + + /** + * Update the estimated state based on measured values. The + * time step is assumed to remain constant. + */ + void update(const Eigen::VectorXd& y); + + /** + * Update the estimated state based on measured values, + * using the given time step and dynamics matrix. + */ + void update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A); + + /** + * Return the current state and time. + */ + Eigen::VectorXd state() { return x_hat; }; + double time() { return t; }; + +private: + + // Matrices for computation + Eigen::MatrixXd A, C, Q, R, P, K, P0; + + // System dimensions + int m, n; + + // Initial and current time + double t0, t; + + // Discrete time step + double dt; + + // Is the filter initialized? + bool initialized; + + // n-size identity + Eigen::MatrixXd I; + + // Estimated states + Eigen::VectorXd x_hat, x_hat_new; +}; diff --git a/src/Algorithms/Libraries/kalman/src/kalman-test.cpp b/src/Algorithms/Libraries/kalman/src/kalman-test.cpp new file mode 100755 index 0000000..fa2cdda --- /dev/null +++ b/src/Algorithms/Libraries/kalman/src/kalman-test.cpp @@ -0,0 +1,77 @@ +/** + * Test for the KalmanFilter class with 1D projectile motion. + * + * @author: Hayk Martirosyan + * @date: 2014.11.15 + */ + +#include +#include +#include + +#include + +int main(int argc, char* argv[]) { + + int n = 3; // Number of states + int m = 1; // Number of measurements + + double dt = 1.0/30; // Time step + + Eigen::MatrixXd A(n, n); // System dynamics matrix + Eigen::MatrixXd C(m, n); // Output matrix + Eigen::MatrixXd Q(n, n); // Process noise covariance + Eigen::MatrixXd R(m, m); // Measurement noise covariance + Eigen::MatrixXd P(n, n); // Estimate error covariance + + // Discrete LTI projectile motion, measuring position only + A << 1, dt, 0, 0, 1, dt, 0, 0, 1; + C << 1, 0, 0; + + // Reasonable covariance matrices + Q << .05, .05, .0, .05, .05, .0, .0, .0, .0; + R << 5; + P << .1, .1, .1, .1, 10000, 10, .1, 10, 100; + + std::cout << "A: \n" << A << std::endl; + std::cout << "C: \n" << C << std::endl; + std::cout << "Q: \n" << Q << std::endl; + std::cout << "R: \n" << R << std::endl; + std::cout << "P: \n" << P << std::endl; + + // Construct the filter + KalmanFilter kf(dt,A, C, Q, R, P); + + // List of noisy position measurements (y) + std::vector measurements = { + 1.04202710058, 1.10726790452, 1.2913511148, 1.48485250951, 1.72825901034, + 1.74216489744, 2.11672039768, 2.14529225112, 2.16029641405, 2.21269371128, + 2.57709350237, 2.6682215744, 2.51641839428, 2.76034056782, 2.88131780617, + 2.88373786518, 2.9448468727, 2.82866600131, 3.0006601946, 3.12920591669, + 2.858361783, 2.83808170354, 2.68975330958, 2.66533185589, 2.81613499531, + 2.81003612051, 2.88321849354, 2.69789264832, 2.4342229249, 2.23464791825, + 2.30278776224, 2.02069770395, 1.94393985809, 1.82498398739, 1.52526230354, + 1.86967808173, 1.18073207847, 1.10729605087, 0.916168349913, 0.678547664519, + 0.562381751596, 0.355468474885, -0.155607486619, -0.287198661013, -0.602973173813 + }; + + // Best guess of initial states + Eigen::VectorXd x0(n); + double t = 0; + x0 << measurements[0], 0, -9.81; + kf.init(t, x0); + + // Feed measurements into filter, output estimated states + + Eigen::VectorXd y(m); + std::cout << "t = " << t << ", " << "x_hat[0]: " << kf.state().transpose() << std::endl; + for(int i = 0; i < measurements.size(); i++) { + t += dt; + y << measurements[i]; + kf.update(y); + std::cout << "t = " << t << ", " << "y[" << i << "] = " << y.transpose() + << ", x_hat[" << i << "] = " << kf.state().transpose() << std::endl; + } + + return 0; +} diff --git a/src/Algorithms/Libraries/kalman/src/kalman.cpp b/src/Algorithms/Libraries/kalman/src/kalman.cpp new file mode 100755 index 0000000..30c5345 --- /dev/null +++ b/src/Algorithms/Libraries/kalman/src/kalman.cpp @@ -0,0 +1,65 @@ +/** +* Implementation of KalmanFilter class. +* +* @author: Hayk Martirosyan +* @date: 2014.11.15 +*/ + +#include +#include + +#include + +KalmanFilter::KalmanFilter( + double dt, + const Eigen::MatrixXd& A, + const Eigen::MatrixXd& C, + const Eigen::MatrixXd& Q, + const Eigen::MatrixXd& R, + const Eigen::MatrixXd& P) + : A(A), C(C), Q(Q), R(R), P0(P), + m(C.rows()), n(A.rows()), dt(dt), initialized(false), + I(n, n), x_hat(n), x_hat_new(n) +{ + I.setIdentity(); +} + +KalmanFilter::KalmanFilter() {} + +void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) { + x_hat = x0; + P = P0; + this->t0 = t0; + t = t0; + initialized = true; +} + +void KalmanFilter::init() { + x_hat.setZero(); + P = P0; + t0 = 0; + t = t0; + initialized = true; +} + +void KalmanFilter::update(const Eigen::VectorXd& y) { + + if(!initialized) + throw std::runtime_error("Filter is not initialized!"); + + x_hat_new = A * x_hat; + P = A*P*A.transpose() + Q; + K = P*C.transpose()*(C*P*C.transpose() + R).inverse(); + x_hat_new += K * (y - C*x_hat_new); + P = (I - K*C)*P; + x_hat = x_hat_new; + + t += dt; +} + +void KalmanFilter::update(const Eigen::VectorXd& y, double dt, const Eigen::MatrixXd A) { + + this->A = A; + this->dt = dt; + update(y); +} diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt new file mode 100644 index 0000000..46560bf --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.10) +project(mkt_algorithm VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# Find dependencies +find_package(Boost REQUIRED COMPONENTS system) +find_package(Eigen3 REQUIRED) + +# Thư mục include +include_directories( + ${PROJECT_SOURCE_DIR}/include + ${EIGEN3_INCLUDE_DIRS} +) + +# Dependencies packages +set(PACKAGES_DIR + geometry_msgs + score_algorithm + nav_2d_msgs + nav_2d_utils + kalman + angles + nav_grid + costmap_2d + sensor_msgs + visualization_msgs + std_msgs +) + +# Tìm tất cả file source cho diff library +file(GLOB DIFF_SOURCES + "src/diff/diff_predictive_trajectory.cpp" + "src/diff/diff_rotate_to_goal.cpp" + "src/diff/diff_go_straight.cpp" +) + +file(GLOB DIFF_HEADERS + "include/mkt_algorithm/diff/*.h" +) + +# Tạo thư viện shared cho diff +add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS}) + +# Link libraries +target_link_libraries(mkt_algorithm_diff + PUBLIC + ${PACKAGES_DIR} + Boost::system + Eigen3::Eigen +) + +# Set include directories +target_include_directories(mkt_algorithm_diff + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} +) + +# Cài đặt header files +install(DIRECTORY include/mkt_algorithm + DESTINATION include + FILES_MATCHING PATTERN "*.h" +) + +# Cài đặt library +install(TARGETS mkt_algorithm_diff + EXPORT mkt_algorithm-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +# Cài đặt plugins.xml (nếu cần cho pluginlib) +if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml) + install(FILES plugins.xml + DESTINATION share/${PROJECT_NAME} +) +endif() + +# Export targets +install(EXPORT mkt_algorithm-targets + FILE mkt_algorithm-targets.cmake + DESTINATION lib/cmake/mkt_algorithm +) + +# Tùy chọn build +option(BUILD_SHARED_LIBS "Build shared libraries" ON) +option(BUILD_TESTS "Build test programs" OFF) + +# Flags biên dịch +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Eigen3 found: ${EIGEN3_FOUND}") +message(STATUS "=================================") diff --git a/src/Algorithms/Libraries/mkt_algorithm/README.md b/src/Algorithms/Libraries/mkt_algorithm/README.md new file mode 100644 index 0000000..e69de29 diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h new file mode 100644 index 0000000..df33bbd --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h @@ -0,0 +1,108 @@ +#ifndef _NAV_ALGORITHM_BICYCLE_H_INCLUDED__ +#define _NAV_ALGORITHM_BICYCLE_H_INCLUDED__ + +#include +#include +#include +#include +#include +#include + +namespace mkt_algorithm +{ + +/** + * @class Bicycle + * @brief Standard Bicycle-like ScoreAlgorithm +*/ +class Bicycle : public score_algorithm::ScoreAlgorithm +{ +public: + + Bicycle() {}; //: line_generator_(nullptr) {}; + + virtual ~Bicycle() {}; + + /** + * @brief Initialize parameters as needed + * @param nh NodeHandle to read parameters from + */ + virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; + + /** + * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories + * + * Subclasses may overwrite. Return false in case there is any error. + * + * @param pose Current pose (costmap frame) + * @param velocity Current velocity + * @param goal The final goal (costmap frame) + * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points + */ + virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + double& x_direction, double& y_direction, double& theta_direction) override; + + /** + * @brief Reset all data + */ + virtual void reset() override; + + /** + * @brief Calculating algorithm + * @param pose + * @param velocity + */ + virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override; + + +protected: + + unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector &plan, + const double distance, unsigned int start_index, unsigned int last_valid_index) const; + + double journey(const std::vector &plan, const unsigned int start_index, const unsigned int last_valid_index) const; + + + bool getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Twist2D& velocity, + const nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, + double &target_direction); + + bool findSubGoalIndex(const std::vector &plan, std::vector &index_s); + + ros::NodeHandle nh_priv_, nh_; + std::string frame_id_path_; + ros::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_; + + std::vector reached_intermediate_goals_; + geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; + geometry_msgs::Pose2D goal_; + nav_2d_msgs::Path2D global_plan_; + + unsigned int start_index_saved_; + unsigned int sub_goal_index_saved_, sub_start_index_saved_; + std::vector start_index_saved_vt_; + int index_samples_; + + double steering_fix_wheel_distance_x_; + double steering_fix_wheel_distance_y_; + double xy_local_goal_tolerance_; + double xy_goal_tolerance_; + double angle_threshold_; + double acc_lim_theta_; + double decel_lim_theta_; + double x_direction_, y_direction_, theta_direction_; + double k_d_reduce_, distance_rg_, k_ld_, ld_min_, ld_max_; + double controller_frequency_param_name_; + + bool follow_step_path_; + bool is_filter_; + boost::shared_ptr kf_; + ros::Publisher cmd_raw_pub_; + ros::Time last_actuator_update_; +}; // class Bicycle + +} // namespace mkt_algorithm + + +#endif //_NAV_ALGORITHM_BICYCLE_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h new file mode 100644 index 0000000..0ca788e --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h @@ -0,0 +1,74 @@ +#ifndef __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__ +#define __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__ +#include +#include +#include + +namespace mkt_algorithm +{ + +/** + * @class NoSolution + */ +class NoSolution : public std::exception { + public: + const char * what () const throw () { + return "Robot cannot calculate error line!"; + } +}; // class NoSolution + +/** + * @class LineGenerator + */ +class LineGenerator +{ + /* data */ + struct LineEquation { + geometry_msgs::Pose2D Gi; /* 𝑮𝒊(𝒙𝒊,𝒚𝒊) */ + geometry_msgs::Pose2D Gj; /* 𝑮𝒋(𝒙𝒋,𝒚𝒋) */ + float a, b, c ; /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */ + }; + + /* Save data present position */ + struct PresentPosition { + float Gxh, Gyh; /* 𝑮𝒋(𝒙h,𝒚h) The head position in Robot */ + float Yaw_degree; /* Angle of Robot (degree) */ + float Yaw_rad; /* Angle of Robot (degree) */ + float error_line; /* 𝒆(𝑮,𝒍𝒊𝒏𝒆)=|𝒂𝒙+𝒃𝒚+𝒄|/sqrt(𝒂^2+𝒃^2) */ + double distance_to_goal; /* The distance from query pose to goal */ + double distance_to_start; /* The distance from query pose to start*/ + double distace_start_goal; + }; +public: + + /** + * @brief To caculated the equation of the line + * @param start_point start point of the line + * @param end_point end point of the line + */ + virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point); + + /** + * @brief Calculating error + * @param[in] Lm The distance from the center of the wheel axis to the center line + * @param[in] pose + * @param[in] rev + */ + virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false); + + /** + * @brief Calculating tolerance + * @param query_pose The pose to check + * @param goal_pose The pose to check against + */ + virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose); + + /* Properties */ + LineEquation Leq_; + PresentPosition present_pose_; + +}; // class LineGenerator + +} // namespace mkt_algorithm + +#endif // __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h new file mode 100644 index 0000000..ba58a0a --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h @@ -0,0 +1,70 @@ +#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__ +#define _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__ + +#include +#include +#include +#include + +namespace mkt_algorithm +{ + +/** + * @class GoStraight + * @brief Standard Bicycle-like ScoreAlgorithm +*/ +class GoStraight : public score_algorithm::ScoreAlgorithm +{ +public: + + GoStraight() {}; + + virtual ~GoStraight() {}; + + /** + * @brief Initialize parameters as needed + * @param nh NodeHandle to read parameters from + */ + virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; + + /** + * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories + * + * Subclasses may overwrite. Return false in case there is any error. + * + * @param pose Current pose (costmap frame) + * @param velocity Current velocity + * @param goal The final goal (costmap frame) + * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points + */ + virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + double& x_direction, double& y_direction, double& theta_direction) override; + + /** + * @brief Reset all data + */ + virtual void reset() override; + + /** + * @brief Calculating algorithm + * @param pose + * @param velocity + */ + virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override; + + +protected: + ros::NodeHandle nh_; + ros::NodeHandle nh_kinematics_; + + double steering_fix_wheel_distance_x_; + double steering_fix_wheel_distance_y_; + geometry_msgs::Pose2D goal_; + ros::Time time_last_cmd_; + +}; // class GoStraight + +} // namespace mkt_algorithm + +#endif \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h new file mode 100644 index 0000000..1ec8602 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h @@ -0,0 +1,45 @@ +#ifndef _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__ +#define _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__ + +#include +#include + +namespace mkt_algorithm +{ +class GoalChecker : public score_algorithm::GoalChecker +{ +public: + + /** + * @brief Initialize any parameters from the NodeHandle + * @param nh NodeHandle for grabbing parameters + */ + virtual void initialize(const ros::NodeHandle& nh) override; + + /** + * @brief Reset the state of the goal checker (if any) + */ + virtual void reset() override; + + /** + * @brief Check whether the goal should be considered reached + * @param query_pose The pose to check + * @param goal_pose The pose to check against + * @param velocity The robot's current velocity + * @return True if goal is reached + */ + bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose, + const std::vector& path, const nav_2d_msgs::Twist2D& velocity) override; + +protected: + + boost::shared_ptr line_generator_; + double yaw_goal_tolerance_; + double xy_goal_tolerance_; + double old_xy_goal_tolerance_; + bool setup_; +}; + +} // namespace mkt_algorithm + +#endif // _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h new file mode 100644 index 0000000..f8f6acd --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h @@ -0,0 +1,60 @@ +#ifndef _NAV_ALGORITHM_PTA_H_INCLUDED__ +#define _NAV_ALGORITHM_PTA_H_INCLUDED__ + +#include +#include + +namespace mkt_algorithm +{ + +/** + * @class PTA + * @brief Standard PTA-like ScoreAlgorithm +*/ +class PTA : public score_algorithm::ScoreAlgorithm +{ +public: + + PTA() : line_generator_(nullptr) {}; + + virtual ~PTA() {}; + +// Standard ScoreAlgorithm Interface + /** + * @brief Initialize parameters as needed + * @param nh NodeHandle to read parameters from + */ + virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; + + /** + * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories + * + * Subclasses may overwrite. Return false in case there is any error. + * + * @param pose Current pose (costmap frame) + * @param velocity Current velocity + * @param goal The final goal (costmap frame) + * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points + */ + virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + double& x_direction, double& y_direction, double& theta_direction) override; + + /** + * @brief Calculating algorithm + * @param pose + * @param velocity + */ + virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override; + +protected: + + boost::shared_ptr line_generator_; + double pta_amplitude_max_; // (Default: 0.5 - Range: 0.1 -> 1.0) Cai dat bien do (mot phia - m) cho phep robot di chuyen trong do + +}; // class PTA + +} // namespace mkt_algorithm + + +#endif //_NAV_ALGORITHM_PTA_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h new file mode 100644 index 0000000..ebaf309 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h @@ -0,0 +1,74 @@ +#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__ +#define _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__ + +#include +#include +#include +#include + +namespace mkt_algorithm +{ + +/** + * @class GoStraight + * @brief Standard Bicycle-like ScoreAlgorithm +*/ +class RotateToGoal : public score_algorithm::ScoreAlgorithm +{ +public: + + RotateToGoal() {}; + + virtual ~RotateToGoal() {}; + + /** + * @brief Initialize parameters as needed + * @param nh NodeHandle to read parameters from + */ + virtual void initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; + + /** + * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories + * + * Subclasses may overwrite. Return false in case there is any error. + * + * @param pose Current pose (costmap frame) + * @param velocity Current velocity + * @param goal The final goal (costmap frame) + * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points + */ + virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + double& x_direction, double& y_direction, double& theta_direction) override; + + /** + * @brief Reset all data + */ + virtual void reset() override; + + /** + * @brief Calculating algorithm + * @param pose + * @param velocity + */ + virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override; + + +protected: + ros::NodeHandle nh_; + ros::NodeHandle nh_kinematics_; + geometry_msgs::Pose2D sub_goal_; + + double steering_fix_wheel_distance_x_; + double steering_fix_wheel_distance_y_; + double angle_threshold_; + double current_direction_; + double velocity_rotate_min_, velocity_rotate_max_; + geometry_msgs::Pose2D goal_; + ros::Time time_last_cmd_; + +}; // class RotateToGoal + +} // namespace mkt_algorithm + +#endif \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h new file mode 100644 index 0000000..4b2cbf1 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h @@ -0,0 +1,43 @@ +#ifndef _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__ +#define _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__ + +#include + +namespace mkt_algorithm +{ + + namespace diff + { + /** + * @class GoStraight + * @brief Standard Bicycle-like ScoreAlgorithm + */ + class GoStraight : public mkt_algorithm::diff::PredictiveTrajectory + { + public: + GoStraight() {}; + + virtual ~GoStraight() {}; + + /** + * @brief Initialize parameters as needed + * @param nh NodeHandle to read parameters from + */ + virtual void initialize( + ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + + /** + * @brief Calculating algorithm + * @param pose + * @param velocity + * @param traj + */ + virtual mkt_msgs::Trajectory2D calculator( + const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + }; // class GoStraight + } // namespace diff + +} // namespace mkt_algorithm + +#endif // _NAV_ALGORITHM_DIFF_ROTATE_TO_GOAL_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h new file mode 100644 index 0000000..833db5e --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -0,0 +1,325 @@ +#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ +#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ + +#include + +#include +#include +#include +#include +// #include +// #include + +// #include +// #include +// #include +// #include +// #include +// #include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace mkt_algorithm +{ + namespace diff + { + /** + * @class PredictiveTrajectory + * @brief Standard PredictiveTrajectory-like ScoreAlgorithm + */ + class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm + { + public: + PredictiveTrajectory() : initialized_(false), nav_stop_(false) {}; + + virtual ~PredictiveTrajectory(); + + // Standard ScoreAlgorithm Interface + /** + * @brief Initialize parameters as needed + * @param nh NodeHandle to read parameters from + */ + virtual void initialize( + ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + + /** + * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories + * + * Subclasses may overwrite. Return false in case there is any error. + * + * @param pose Current pose (costmap frame) + * @param velocity Current velocity + * @param goal The final goal (costmap frame) + * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points + */ + virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, + const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, + double &x_direction, double &y_direction, double &theta_direction) override; + + /** + * @brief Calculating algorithm + * @param pose + * @param velocity + * @param traj + */ + virtual mkt_msgs::Trajectory2D calculator( + const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Reset all data + */ + virtual void reset() override; + + /** + * @brief Stoping move navigation + */ + virtual void stop() override; + + /** + * @brief resume move navigation after stopped + */ + virtual void resume() override; + + protected: + inline double sign(double x) + { + return x < 0.0 ? -1.0 : 1.0; + } + + /** + * @brief Initialize parameters + */ + virtual void getParams(); + + /** + * @brief Initialize dynamic reconfigure + * @param nh NodeHandle to read parameters from + */ + virtual void initDynamicReconfigure(const ros::NodeHandle &nh); + + /** + * @brief Dynamically adjust look ahead distance based on the speed + * @param velocity + * @return look ahead distance + */ + double getLookAheadDistance(const nav_2d_msgs::Twist2D &velocity); + + /** + * @brief Get lookahead point on the global plan + * @param lookahead_dist + * @param global_plan + * @return lookahead point + */ + std::vector::iterator + getLookAheadPoint(const nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, nav_2d_msgs::Path2D global_plan); + + /** + * @brief Create carrot message + * @param carrot_pose + * @return carrot message + */ + geometry_msgs::PointStamped createCarrotMsg(const nav_2d_msgs::Pose2DStamped &carrot_pose); + + /** + * @brief Prune global plan + * @param tf + * @param pose + * @param global_plan + * @param dist_behind_robot + * @return true if plan is pruned, false otherwise + */ + bool pruneGlobalPlan(TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose, + nav_2d_msgs::Path2D &global_plan, double dist_behind_robot); + + /** + * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). + * + * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h + * such that the index of the current goal pose is returned as well as + * the transformation between the global plan and the planning frame. + * @param tf A reference to a tf buffer + * @param global_plan The plan to be transformed + * @param pose The pose of the robot + * @param costmap A reference to the costmap being used so the window size for transforming can be computed + * @param global_frame The frame to transform the plan to + * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!] + * @param[out] transformed_plan Populated with the transformed plan + * @return \c true if the global plan is transformed, \c false otherwise + */ + bool transformGlobalPlan( + TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose, + const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length, + nav_2d_msgs::Path2D &transformed_plan); + + /** + * @brief Should rotate to path + * @param carrot_pose + * @param angle_to_path + * @return true if should rotate, false otherwise + */ + bool shouldRotateToPath( + const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &carrot_pose, const nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x); + + /** + * @brief Rotate to heading + * @param angle_to_path + * @param velocity The velocity of the robot + * @param cmd_vel The velocity commands to be filled + */ + void rotateToHeading(const double &angle_to_path, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel); + + /** + * @brief the robot is moving acceleration limits + * @param velocity The velocity of the robot + * @param cmd_vel The velocity commands + * @param cmd_vel_result The velocity commands result + */ + void moveWithAccLimits( + const nav_2d_msgs::Twist2D &velocity, const nav_2d_msgs::Twist2D &cmd_vel, nav_2d_msgs::Twist2D &cmd_vel_result); + + /** + * @brief Stop the robot taking into account acceleration limits + * @param pose The pose of the robot in the global frame + * @param velocity The velocity of the robot + * @param cmd_vel The velocity commands to be filled + * @return True if a valid trajectory was found, false otherwise + */ + bool stopWithAccLimits(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel); + + /** + * @brief Apply constraints + * @param dist_error + * @param lookahead_dist + * @param curvature + * @param curr_speed + * @param pose_cost + * @param linear_vel + * @param sign + */ + void applyConstraints( + const double &dist_error, const double &lookahead_dist, + const double &curvature, const nav_2d_msgs::Twist2D &curr_speed, + const double &pose_cost, double &linear_vel, double &sign_x); + + std::vector interpolateFootprint(geometry_msgs::Pose2D pose, std::vector footprint, double resolution); + + /** + * @brief Cost at pose + * @param x + * @param y + * @return cost + */ + double costAtPose(const double &x, const double &y); + + void updateCostAtOffset( + TFListenerPtr tf, const std::string &robot_base_frame, const nav_2d_msgs::Pose2DStamped &base_pose, + double x_offset, double y_offset, double &cost_left, double &cost_right); + + double computeDecelerationFactor(double remaining_distance, double decel_distance); + + double getEffectiveDistance(const nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan); + + bool detectWobbleByCarrotAngle(std::vector& angle_history, double current_angle, + double amplitude_threshold = 0.3, size_t window_size = 20); + + void publishMarkers(nav_2d_msgs::Pose2DStamped pose); + + std::vector angle_history_; + size_t window_size_; + + bool initialized_; + bool nav_stop_, avoid_obstacles_; + ros::NodeHandle nh_, nh_priv_; + std::string frame_id_path_; + std::string robot_base_frame_; + ros::Publisher carrot_pub_; + + nav_2d_msgs::Pose2DStamped goal_; + nav_2d_msgs::Path2D global_plan_; + nav_2d_msgs::Path2D compute_plan_; + nav_2d_msgs::Path2D transform_plan_; + nav_2d_msgs::Twist2D prevous_drive_cmd_; + ros::Publisher drive_pub_; + + double x_direction_, y_direction_, theta_direction_; + double max_robot_pose_search_dist_; + double global_plan_prune_distance_{1.0}; + + // Lookahead + bool use_velocity_scaled_lookahead_dist_; + double lookahead_time_; + double lookahead_dist_; + double min_lookahead_dist_; + double max_lookahead_dist_; + + // journey + double min_journey_squared_{1e9}; + double max_journey_squared_{1e9}; + + // Rotate to heading + bool use_rotate_to_heading_; + double rotate_to_heading_min_angle_; + + double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_; + double min_path_distance_, max_path_distance_; + + // Regulated linear velocity scaling + bool use_regulated_linear_velocity_scaling_; + double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_; + double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_; + + double rot_stopped_velocity_, trans_stopped_velocity_; + + double min_approach_linear_velocity_; + double regulated_linear_scaling_min_radius_; + double regulated_linear_scaling_min_speed_; + + bool use_cost_regulated_linear_velocity_scaling_; + double inflation_cost_scaling_factor_; + double cost_scaling_dist_, cost_scaling_gain_; + + double cost_left_goal_, cost_right_goal_; + double cost_left_side_ , cost_right_side_; + double center_cost_; + + // Control frequency + double control_duration_; + + std::shared_ptr ddr_; + base_local_planner::BaseLocalPlannerConfig config_; + + base_local_planner::WorldModel *world_model_; ///< @brief The world model that the controller will use + base_local_planner::TrajectoryPlanner *tc_; ///< @brief The trajectory controller + base_local_planner::MapGridVisualizer map_viz_; + std::vector footprint_spec_; + + ros::Time last_actuator_update_; + boost::shared_ptr kf_; + int m_, n_; + Eigen::MatrixXd A; + Eigen::MatrixXd C; + Eigen::MatrixXd Q; + Eigen::MatrixXd R; + Eigen::MatrixXd P; + + ros::Publisher cost_right_goals_pub_, cost_left_goals_pub_; + // visualization_msgs::Marker L_, R_; + }; // class PredictiveTrajectory + + } // namespace diff + +} // namespace mkt_algorithm + +#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h new file mode 100644 index 0000000..5d21ef7 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h @@ -0,0 +1,68 @@ +#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__ +#define _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__ + +#include + +namespace mkt_algorithm +{ + namespace diff + { + /** + * @class GoStraight + * @brief Standard Bicycle-like ScoreAlgorithm + */ + class RotateToGoal : public mkt_algorithm::diff::PredictiveTrajectory + { + public: + RotateToGoal() {}; + + virtual ~RotateToGoal() {}; + + /** + * @brief Initialize parameters as needed + * @param nh NodeHandle to read parameters from + */ + virtual void initialize( + ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + + /** + * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories + * + * Subclasses may overwrite. Return false in case there is any error. + * + * @param pose Current pose (costmap frame) + * @param velocity Current velocity + * @param goal The final goal (costmap frame) + * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points + */ + virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, + const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, + double &x_direction, double &y_direction, double &theta_direction) override; + + /** + * @brief Reset all data + */ + virtual void reset() override; + + /** + * @brief Calculating algorithm + * @param pose + * @param velocity + * @param traj + */ + virtual mkt_msgs::Trajectory2D calculator( + const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + + protected: + /** + * @brief Initialize parameters + */ + virtual void getParams() override; + + }; // class RotateToGoalDiff + + } // namespace diff + +} // namespace mkt_algorithm + +#endif // _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/package.xml b/src/Algorithms/Libraries/mkt_algorithm/package.xml new file mode 100644 index 0000000..3cb27e7 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/package.xml @@ -0,0 +1,94 @@ + + + mkt_algorithm + 0.0.0 + The mkt_algorithm package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + score_algorithm + nav_2d_msgs + roscpp + sensor_msgs + nav_2d_utils + kalman + ddynamic_reconfigure + costmap_2d + base_local_planner + visualization_msgs + + geometry_msgs + score_algorithm + nav_2d_msgs + roscpp + sensor_msgs + nav_2d_utils + kalman + ddynamic_reconfigure + costmap_2d + base_local_planner + visualization_msgs + + geometry_msgs + score_algorithm + nav_2d_msgs + roscpp + sensor_msgs + nav_2d_utils + kalman + ddynamic_reconfigure + costmap_2d + base_local_planner + visualization_msgs + + pluginlib + + + + + + diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp new file mode 100644 index 0000000..ca11d0d --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp @@ -0,0 +1,68 @@ +#include +#include +#include +#include + +namespace mkt_algorithm +{ + +void GoStraight::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) +{ + nh_ = ros::NodeHandle(nh, name); + name_ = name; + tf_ = tf; + costmap_ros_ = costmap_ros; + + steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1); + steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.); +} + +void GoStraight::reset() +{ + time_last_cmd_ = ros::Time::now(); +} + +bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + double& x_direction, double& y_direction, double& theta_direction) +{ + goal_ = goal; + + const double rb_direction_x = goal_.x - pose.x; + const double rb_direction_y = goal_.y - pose.y; + const double rb_angle = atan2(rb_direction_y, rb_direction_x); + double rb_dir = cos(rb_angle- pose.theta); + x_direction = y_direction = theta_direction = rb_dir; + + return true; +} + +nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) +{ + nav_2d_msgs::Twist2DStamped result; + result.header.stamp = ros::Time::now(); + + geometry_msgs::Pose2D steer_to_baselink_pose; + + if(!getPose("steer_link", "base_link", steer_to_baselink_pose)) + return result; + double odom_alpha = steer_to_baselink_pose.theta; + if(fabs(odom_alpha) > 0.03) return result; + + double delta_x = goal_.x - pose.x; + double delta_y = goal_.y - pose.y; + + double d_rg = sqrt(pow(delta_x, 2) + pow(delta_y, 2)); + double d_reduce = std::min(std::max(fabs(velocity.x), 0.6), 0.9); + double v_min = 0.03 * velocity.x / (fabs(velocity.x) + 1e-9); + +// Van toc robot + double vr = d_rg > d_reduce? velocity.x : (velocity.x - v_min)*sin((d_rg / d_reduce) * (M_PI/2)) + v_min; + result.velocity.x = fabs(cos(fabs(0.0 - odom_alpha))) * vr / cos(std::min(M_PI/3, fabs(odom_alpha))); + // ROS_INFO("d_rg %f d_reduce %f V = %f %f", d_rg, d_reduce,velocity.x, vr); + return result; +} + +}; // namespace mkt_algorithm + +PLUGINLIB_EXPORT_CLASS(mkt_algorithm::GoStraight, score_algorithm::ScoreAlgorithm) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp new file mode 100644 index 0000000..eeb4359 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp @@ -0,0 +1,641 @@ +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace mkt_algorithm +{ + +void Bicycle::initialize(const ros::NodeHandle& nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) +{ + nh_priv_ = ros::NodeHandle(nh, name); + nh_ = ros::NodeHandle("~"); + name_ = name; + tf_ = tf; + costmap_ros_ = costmap_ros; + last_actuator_update_ = ros::Time(0); + + nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); + nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5); + nh_priv_.param("angle_threshold", angle_threshold_, M_PI/8); + nh_priv_.param("index_samples", index_samples_, 0); + nh_priv_.param("filter", is_filter_, false); + + steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_x", 1.1); + steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_y", 0.); + xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh_priv_, "xy_goal_tolerance", 0.1); + acc_lim_theta_ = nav_2d_utils::searchAndGetParam(nh_priv_, "acc_lim_theta", 0.1); + decel_lim_theta_ = nav_2d_utils::searchAndGetParam(nh_priv_, "decel_lim_theta", 0.1); + controller_frequency_param_name_ = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); + if(controller_frequency_param_name_ <= 0.0) + { + ROS_WARN("controller_frequency is not setting lower 0.0 so will set to 1.0"); + controller_frequency_param_name_ = 1.0; + } + + reached_intermediate_goals_pub_ = nh_.advertise("reached_intermediate_goals", 1); + current_goal_pub_ = nh_.advertise("current_goal", 1); + closet_robot_goal_pub_ = nh_.advertise("closet_robot_goal", 1); + cmd_raw_pub_ = nh_.advertise("cmd_raw", 1); + x_direction_ = y_direction_= theta_direction_ = 0; + +// kalman + int n = 3; // Number of states + int m = 1; // Number of measurements + double dt = 1.0/controller_frequency_param_name_; // Time step + // ROS_INFO("controller_frequency_param_name %f", controller_frequency_param_name); + Eigen::MatrixXd A(n, n); // System dynamics matrix + Eigen::MatrixXd C(m, n); // Output matrix + Eigen::MatrixXd Q(n, n); // Process noise covariance + Eigen::MatrixXd R(m, m); // Measurement noise covariance + Eigen::MatrixXd P(n, n); // Estimate error covariance + + // Discrete LTI projectile motion, measuring position only + A << 1, dt, 0, 0, 1, dt, 0, 0, 1; + C << 1, 0, 0; + + // Reasonable covariance matrices + double q = 0.1; + double r = 0.6; + double p = 0.2; + Q << q, q, 0.0, q, q, 0.0, 0.0, 0.0, 0.0; + R << r; + P << p, p, p, p, 10.0, 1, p, 1, 10.0; + + // Construct the filter + kf_ = boost::make_shared(dt,A, C, Q, R, P); + // Best guess of initial states + Eigen::VectorXd x0(n); + x0 << 0.0, 0.0, 0.0; + kf_->init(ros::Time::now().toSec(), x0); +} + +void Bicycle::reset() +{ + reached_intermediate_goals_.clear(); + start_index_saved_vt_.clear(); + start_index_saved_ = 0; + sub_goal_index_saved_ = 0; + sub_start_index_saved_ = 0; + x_direction_ = y_direction_= theta_direction_ = 0; + follow_step_path_ = false; + last_actuator_update_ = ros::Time(0); +} + +bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + double& x_direction, double& y_direction, double& theta_direction) +{ + nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); + nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5); + nh_priv_.param("angle_threshold", angle_threshold_, M_PI/8); + nh_priv_.param("index_samples", index_samples_, 0); + nh_priv_.param("distance_rg", distance_rg_, 0.1); + nh_priv_.param("k_ld", k_ld_, 1.0); + nh_priv_.param("k_d_reduce", k_d_reduce_, 1.0); + nh_priv_.param("ld_min", ld_min_, 0.2); + nh_priv_.param("ld_max", ld_max_, 1.1); + nh_priv_.param("follow_step_path", follow_step_path_, false); + + // ROS_INFO("%f %f %f %d", xy_local_goal_tolerance_, xy_local_goal_tolerance_offset_, angle_threshold_, index_samples_); + frame_id_path_ = global_plan.header.frame_id; + goal_ = goal; + global_plan_ = global_plan; + double rb_direction; + if(!getGoalPose(pose, velocity,global_plan, sub_goal_, rb_direction)) + { + ROS_ERROR("getGoalPose Failed"); + return false; + } + x_direction_= fabs(rb_direction)/rb_direction; + x_direction = x_direction_; + y_direction = y_direction_ = 0; + theta_direction = theta_direction_; + return true; +} + +nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) +{ + nav_2d_msgs::Twist2DStamped result; + result.header.stamp = ros::Time::now(); + + geometry_msgs::Pose2D steer_to_baselink_pose; + + if(!getPose("steer_link", "base_link", steer_to_baselink_pose)) + return result; + double actual_phi_steer = steer_to_baselink_pose.theta; + + const double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2)); + const double delta_x = sub_goal_.x - pose.x; + const double delta_y = sub_goal_.y - pose.y; + const double d_rg = sqrt(pow(delta_x, 2) + pow(delta_y, 2)); + +// Van toc robot + double d_reduce = std::min(std::max(fabs(velocity.x) * k_d_reduce_, 0.6), 3.0); + double v_min = 0.05 * velocity.x / (fabs(velocity.x) + 1e-9); + double vr = d_rg > d_reduce? velocity.x : (velocity.x - v_min)*sin((d_rg / d_reduce) * (M_PI/2)) + v_min; + +// Góc lái của bánh lái là: α(t) + double alpha = atan2(delta_y,delta_x) - pose.theta; + double ld = std::min(std::max(fabs(velocity.x)*k_ld_, ld_min_), ld_max_); + // double ld = std::min(std::max(d_rg*k_ld_, ld_min_), ld_max_); + + double phi_steer = atan2(2 * L * sin(alpha), ld); + if(d_rg <= distance_rg_ && global_plan_.poses.size() > 1) + { + const double d_x = goal_.x - sub_goal_.x; + const double d_y = goal_.y - sub_goal_.y; + const double dd = sqrt(d_x * d_x + d_y * d_y); + const double d_theta = goal_.theta - sub_goal_.theta; + if(dd < 1e-5) + { + geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2]; + alpha = velocity.x / (fabs(velocity.x) + 1e-9)*(p.theta - pose.theta); + phi_steer = atan2(2 * L * sin(alpha), 1.0); + } + } + + double alpha_reduce = std::min(std::max(fabs(vr), 0.6), 1.0); + double cos_actual_phi_steer = cos( std::min(M_PI/2.3, fabs(actual_phi_steer))); + + // limit W + double result_W_max = 0.25; + // double result_W_min = 0.13; + double result_W = vr * fabs(cos(alpha)) * tan(actual_phi_steer) / L; + double V_s = std::max(alpha_reduce, fabs(cos(alpha))) * vr / cos_actual_phi_steer; + std::stringstream ss; + if(fabs(result_W) > 0.02) + { + double d_w = fabs(result_W) - fabs(velocity.theta); + double a = fabs(velocity.theta) > fabs(result_W) ? fabs(acc_lim_theta_/controller_frequency_param_name_) : fabs(decel_lim_theta_/controller_frequency_param_name_); + // ss << d_w << " " << a << " "; + double result; + if(fabs(d_w) >= a) + result = fabs(velocity.theta) + d_w / fabs(d_w + 1e-9) * a; + else if (fabs(d_w) < fabs(a)) + result = fabs(velocity.theta); + // ss << result_W << " " << velocity.theta << " "; + result_W = result_W / (fabs(result_W) + 1e-9) * result; + // ss << result_W << " "; + // ROS_INFO_STREAM(ss.str()); + + double d_v = fabs(result_W * L / tan(actual_phi_steer)) - fabs(vr); + if(fabs(d_v) >= a) + result = velocity.x / (fabs(velocity.x) + 1e-9)* (fabs(vr) + d_v / fabs(d_v + 1e-9) * a); + else if (fabs(d_v) < fabs(a)) + result = velocity.x / (fabs(velocity.x) + 1e-9) * fabs(vr); + + V_s = std::max(alpha_reduce, fabs(cos(alpha)))*result/ cos_actual_phi_steer; + } + + if(fabs(result_W) > result_W_max) + V_s = ( result_W / (fabs(result_W) + 1e-9) * result_W_max * L / tan(actual_phi_steer)) / cos_actual_phi_steer; + + if(is_filter_) + { + geometry_msgs::Twist msg; + msg.linear.x = result.velocity.x; + msg.angular.z = phi_steer; + cmd_raw_pub_.publish(msg); + + Eigen::VectorXd y(1); + y << phi_steer; + ros::Time current_time = ros::Time::now(); + double dt = ros::Duration(current_time - last_actuator_update_).toSec(); + last_actuator_update_ = current_time; + Eigen::MatrixXd A(3, 3); // System dynamics matrix + A << 1, dt, 0, 0, 1, dt, 0, 0, 1; + kf_->update(y, dt, A); + phi_steer = kf_->state().transpose()[0]; + } + theta_direction_ = phi_steer !=0 ? phi_steer/(fabs(phi_steer) + 1e-9) * velocity.x/((fabs(velocity.x) + 1e-9)) : 0; + + result.velocity.theta = phi_steer; + result.velocity.x = fabs(cos(phi_steer - actual_phi_steer)) * V_s; + return result; +} + +unsigned int Bicycle::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector &plan, + const double distance, 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); + double S = sqrt(pow(start_direction_x, 2) + pow(start_direction_y, 2)); + // double angle_threshold = 0.0; + 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; + const double dd = sqrt(pow(current_direction_x, 2) + pow(current_direction_y, 2)); + S += dd; + if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + { + double current_angle = atan2(current_direction_y, current_direction_x); + if(fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= angle_threshold_ || S >= distance) + { + break; + } + goal_index = i; + } + } + } + return goal_index; +} + +bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Twist2D& velocity, + const nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, + double &target_direction) +{ + 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; + } + // Chuyển đổi dữ liệu kế hoạch đường đi 'nav_2d_msgs::Path2D' sang 'std::vector' + std::vector plan = global_plan.poses; + + std::vector index_s; + if(!findSubGoalIndex(plan, index_s)) + { + ROS_ERROR_NAMED("PathProgressCritic", "Can not find SubGoal Index"); + return false; + } + // else + // { + // std::stringstream ss; + // for(auto i : index_s) ss << i << " "; + // ROS_INFO_STREAM(ss.str()); + // } + + unsigned int sub_goal_index = sub_goal_index_saved_; + unsigned int sub_start_index = sub_start_index_saved_; + if(index_s.empty() || index_s.size() < 1) + { + sub_goal_index = (unsigned int)plan.size(); + sub_goal_index_saved_ = sub_goal_index; + } + else + { + if(index_s.front() > sub_goal_index_saved_) + { + sub_goal_index = (unsigned int)(index_s.front()) + 1; + sub_goal_index_saved_ = sub_goal_index; + } + if(index_s.size() > 1) + { + for(int i = 1; i < index_s.size(); ++i) + { + // double tolerance = line_generator->calculateTolerance(robot_pose, plan[index_s[i-1]]); + double distance = fabs(nav_2d_utils::poseDistance(robot_pose, plan[index_s[i-1]])); + // double dx = plan[index_s[i-1]].x - robot_pose.x; + // double dy = plan[index_s[i-1]].y - robot_pose.y; + // if(std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_goal_tolerance_ + 0.15) + if (distance < xy_local_goal_tolerance_offset_) + { + if(index_s[i] > sub_goal_index_saved_) + { + if(i < index_s.size()-1) + { + sub_goal_index = index_s[i] + 1; + } + else + { + sub_goal_index = (unsigned int)plan.size(); + } + if(follow_step_path_) + { + sub_start_index = index_s[i-1]; + sub_start_index_saved_ = sub_start_index; + } + ROS_INFO("Tang nac path tu %d den %d boi khoang cach", sub_goal_index_saved_, sub_goal_index); + // ROS_WARN("i %d follow_step_path_ = %x start = %d sub_goal = %d size = %d", i, follow_step_path_, sub_start_index, sub_goal_index, (unsigned int)plan.size()); + sub_goal_index_saved_ = sub_goal_index; + break; + } + } + } + } + else + { + double distance = fabs(nav_2d_utils::poseDistance(plan[sub_goal_index], robot_pose)); + if(distance <= xy_local_goal_tolerance_ ) + { + sub_goal_index = (unsigned int)plan.size(); + sub_goal_index_saved_ = sub_goal_index; + } + } + + } + + // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map + // Tìm điểm pose bắt đầu trên kế hoạch đường đi gần nhất với robot đồng thời có tồn tại trong local map + unsigned int start_index = 0; // số vị trí pose trong kế hoạch đường đi + double distance_to_start = std::numeric_limits::infinity(); // Xét giá trị là vô cùng oo + bool started_path = false; + // for (unsigned int i = 0; i < plan.size(); i++) + // ROS_INFO_THROTTLE(1.0, "follow_step_path_ = %x start = %d sub_goal = %d size = %d", follow_step_path_, sub_start_index, sub_goal_index, (unsigned int)plan.size()); + for (unsigned int i = sub_start_index; i < sub_goal_index; 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) // chuyển tọa độ từ đơn vị mét sang pixel + // && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) + // { + // Still on the costmap. Continue. + double distance = fabs(nav_2d_utils::poseDistance(plan[i], robot_pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét + if (distance_to_start > distance) + { + start_index = i; + distance_to_start = distance; + started_path = true; + } + // else + // { + // break; + // } + // else if (started_path) + // { + // // 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, we have not yet found a point on the costmap, so we just continue + } + // Nếu khồng tìm được điểm gần với robot nhất thì trả về false + 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 + // Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map + if(start_index < 1) start_index == 1; + // unsigned int last_valid_index = plan.size() - 1; + unsigned int last_valid_index = sub_goal_index - 1; + // for (unsigned int i = start_index + 1; i < sub_goal_index; 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. + // Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan + + unsigned int goal_index = start_index; + ros::Rate r(30); + while (goal_index < last_valid_index && ros::ok()) + { + // Tìm kiếm vị trí điểm mục tiêu phù hợp + double S; + if(velocity.x > 0) S = std::min(4.0, std::max(0.9, fabs(velocity.x)*k_d_reduce_)); + // else if(velocity.x < 0) S = 0.5; + else if(velocity.x < 0) S = std::min(0.7, std::max(0.5, fabs(velocity.x)*k_d_reduce_)); + else S = 0.5; + goal_index = getGoalIndex(robot_pose, plan, S, start_index, last_valid_index); + + // check if goal already reached + bool goal_already_reached = false; + + geometry_msgs::PoseArray poseArrayMsg; + poseArrayMsg.header.stamp = ros::Time::now(); + poseArrayMsg.header.frame_id = frame_id_path_; + // std::stringstream ss; + for (auto &reached_intermediate_goal : reached_intermediate_goals_) + { + double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index])); + // ss << distance << " "; + geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(reached_intermediate_goal); + poseArrayMsg.poses.push_back(pose); + } + reached_intermediate_goals_pub_.publish(poseArrayMsg); + // if(!reached_intermediate_goals_.empty()) + // { + // ROS_INFO_STREAM(ss.str()); + // } + + for (auto &reached_intermediate_goal : reached_intermediate_goals_) + { + double distance = fabs(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_offset_) + { + break; + } + } + // start_index_saved_ = start_index; + break; + } + } + + //try hiep + unsigned num = 0; + start_index_saved_vt_.insert(start_index_saved_vt_.begin(),start_index); + if(start_index_saved_vt_.size() > index_samples_) start_index_saved_vt_.pop_back(); + std::stringstream ss; + for(int i = 0; i < start_index_saved_vt_.size() - 1; i++) + { + double edge = 0; + if(start_index_saved_vt_[i] != start_index_saved_vt_[i+1]) + edge = journey(plan, start_index_saved_vt_[i], start_index_saved_vt_[i+1]); + if(edge < xy_local_goal_tolerance_) num++; + ss << start_index_saved_vt_[i] << " "; + } + // ss << start_index << " "; + + if(num < index_samples_ - 1 && !start_index_saved_vt_.empty()) + { + auto max_iter = *(std::max_element(start_index_saved_vt_.begin(), start_index_saved_vt_.end())); + start_index_saved_ = start_index_saved_ >= max_iter? start_index_saved_ : max_iter; + double S1 = journey(plan, start_index, plan.size()-1 ); + double S2 = journey(plan, start_index_saved_, plan.size()-1 ); + + // ROS_INFO_STREAM(ss.str()); + if(S2 - S1 < -fabs(xy_local_goal_tolerance_)) + { + start_index = start_index_saved_; + } + else start_index_saved_ = start_index; + // ROS_INFO(" %d %d %f %d %f %f", max_iter, start_index, S1, start_index_saved_, S2, S2 - S1); + } + else start_index_saved_ = start_index; + + if(start_index >= goal_index) goal_index = getGoalIndex(robot_pose, plan, S,start_index, last_valid_index); + + if (!goal_already_reached) + { + // new goal not in reached_intermediate_goals_ + double distance = fabs(nav_2d_utils::poseDistance(plan[goal_index], plan[start_index])); + if (distance < xy_local_goal_tolerance_) + { + // ROS_INFO("goal_index %d distance %f", goal_index, distance); + // 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; + } + } + + r.sleep(); + ros::spinOnce(); + } + + if (start_index > goal_index) + start_index = goal_index; + ROS_ASSERT(goal_index <= last_valid_index); + if((unsigned int)plan.size() == 2 && !plan.empty()) + { + start_index = 0; + goal_index = 1; + } + + // Tham chiếu lại đối số đầu vao + sub_goal = plan[goal_index]; + if(goal_index > 0) rev_sub_goal_ = plan[goal_index - 1]; + else rev_sub_goal_ = plan[start_index]; + + target_direction = x_direction_; + if(goal_index > 0 && (unsigned int)plan.size() > 1) + { + if(goal_index < (unsigned int)(plan.size() -1)) + { + const geometry_msgs::Pose2D p1 = plan[start_index]; + int index; + for(index = start_index+1; index < goal_index; index++) + { + geometry_msgs::Pose2D pose = plan[index]; + if(nav_2d_utils::poseDistance(p1, pose) > xy_goal_tolerance_) break; + } + const geometry_msgs::Pose2D p2 = plan[index]; + if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + { + const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x); + const double dir_path = cos(fabs(path_angle - p2.theta)); + if(fabs(dir_path) > 1e-9) + target_direction = dir_path; + // ROS_INFO("%f %f %f %f", p1.x ,p1.y, p2.x ,p2.y); + // ROS_INFO("%d %0.2f %0.2f %0.2f", goal_index, path_angle, p2.theta, target_direction); + } + } + else + { + const double rb_direction_x = sub_goal.x - robot_pose.x; + const double rb_direction_y = sub_goal.y - robot_pose.y; + const double rb_angle = atan2(rb_direction_y, rb_direction_x); + double dir_rg = cos(rb_angle - robot_pose.theta); + const double angle_threshold = 0.6; + if(fabs(dir_rg) > angle_threshold) + target_direction = dir_rg; + } + } + + // Publish start_index + geometry_msgs::PoseStamped pose_st = nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, ros::Time::now()); + closet_robot_goal_pub_.publish(pose_st); + + // Publish goal_index + geometry_msgs::PoseStamped pose_g = nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, ros::Time::now()); + current_goal_pub_.publish(pose_g); + + return true; +} + +bool Bicycle::findSubGoalIndex(const std::vector &plan, std::vector &index_s) +{ + if (plan.empty()) + { + return false; + } + double x_direction_saved = 0.0; + geometry_msgs::PoseArray poseArrayMsg; + poseArrayMsg.header.stamp = ros::Time::now(); + poseArrayMsg.header.frame_id = frame_id_path_; + + for (unsigned int i = 1; i < (unsigned int)plan.size(); i++) + { + geometry_msgs::Pose2D p1 = plan[i]; + geometry_msgs::Pose2D p2 = plan[i+1]; + // geometry_msgs::Pose2D p3 = plan[i+2]; + double angle1; + if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + angle1 = atan2(p2.y - p1.y, p2.x - p1.x); + else continue; + // const double angle2 = atan2(p3.y - p2.y, p3.x - p2.x); + const double target_direction = cos(fabs(angle1 - p2.theta)); + + if(target_direction * x_direction_saved < 0.0) + { + index_s.push_back(i); + geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(plan[i]); + poseArrayMsg.poses.push_back(pose); + } + x_direction_saved = target_direction; + } + + if(index_s.empty()) + index_s.push_back((unsigned int)plan.size() - 1); + else + { + if(index_s.back() < (unsigned int)plan.size() - 1) + index_s.push_back((unsigned int)plan.size() - 1); + } + reached_intermediate_goals_pub_.publish(poseArrayMsg); + return true; +} + +double Bicycle::journey(const std::vector &plan, const unsigned int start_index, const unsigned int last_valid_index) const +{ + double S = 0; + if(last_valid_index - start_index > 1) + { + for(int i= start_index; i < last_valid_index; i++) + { + S += nav_2d_utils::poseDistance(plan[i], plan[i+1]); + } + } + return S; +} + + +} // namespace mkt_algorithm + +PLUGINLIB_EXPORT_CLASS(mkt_algorithm::Bicycle, score_algorithm::ScoreAlgorithm) diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp new file mode 100644 index 0000000..c40a715 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp @@ -0,0 +1,78 @@ +#include +#include +#include +#include + +namespace mkt_algorithm +{ + +void RotateToGoal::initialize(const ros::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) +{ + nh_ = ros::NodeHandle(nh, name); + name_ = name; + tf_ = tf; + costmap_ros_ = costmap_ros; + + steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1); + steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.); + nh_.param("velocity_rotate_min", velocity_rotate_min_, 0.1); + nh_.param("velocity_rotate_max", velocity_rotate_max_, 0.6); + nh_.param("angle_pass_rotate", angle_threshold_, 0.02); +} + +void RotateToGoal::reset() +{ + time_last_cmd_ = ros::Time::now(); +} + +bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + double& x_direction, double& y_direction, double& theta_direction) +{ + goal_ = goal; + double angle = angles::shortest_angular_distance(pose.theta, goal_.theta); + + if(angle > 0) current_direction_ = 1; + else if(angle < 0) current_direction_ = -1; + x_direction = y_direction = theta_direction = current_direction_; + return true; +} + +nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) +{ + nav_2d_msgs::Twist2DStamped result; + result.header.stamp = ros::Time::now(); + + geometry_msgs::Pose2D steer_to_baselink_pose; + + if(!getPose("steer_link", "base_link", steer_to_baselink_pose)) + return result; + double actual_phi_steer = steer_to_baselink_pose.theta; + + double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2)); + + // ros::Time t = ros::Time::now(); + // double dt = time_last_cmd_.isZero() ? 0.0 : (t - time_last_cmd_).toSec(); + // time_last_cmd_ = ros::Time::now(); + + double angle_rg = angles::shortest_angular_distance(pose.theta, goal_.theta); + double angle_reduce = std::min(std::max(fabs(velocity.theta), 0.15), velocity_rotate_max_); + double w_min = velocity.theta / (fabs(velocity.theta) + 1e-9) * velocity_rotate_min_; + double w_r = fabs(angle_rg) > angle_reduce? velocity.theta : (velocity.theta - w_min)*sin((fabs(angle_rg) / angle_reduce) * (M_PI/2)) + w_min; + + int direct_rotate = (angle_rg/fabs(angle_rg)); + double target_phi_steer = direct_rotate * M_PI/2; + if( fabs(target_phi_steer - actual_phi_steer) <= angle_threshold_) + { + double target_v = fabs(w_r * L); + result.velocity.x = std::min(velocity_rotate_max_ * L, fabs(target_v)); + } + else result.velocity.x = 0.0; + + result.velocity.theta = target_phi_steer; + return result; +} + +}; // namespace mkt_algorithm + +PLUGINLIB_EXPORT_CLASS(mkt_algorithm::RotateToGoal, score_algorithm::ScoreAlgorithm) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp new file mode 100644 index 0000000..d17c2b9 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -0,0 +1,243 @@ +#include + +// other +// #include +#include + +void mkt_algorithm::diff::GoStraight::initialize( + ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) +{ + if (!initialized_) + { + nh_ = ros::NodeHandle("~/"); + nh_priv_ = ros::NodeHandle("~/" + name); + name_ = name; + tf_ = tf; + traj_ = traj; + costmap_ros_ = costmap_ros; + + this->getParams(); + // this->initDynamicReconfigure(nh_priv_); + nh_.param("publish_topic", enable_publish_, false); + nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); + + + carrot_pub_ = nh_.advertise("lookahead_point", 1); + reached_intermediate_goals_pub_ = nh_.advertise("reached_intermediate_goals", 1); + current_goal_pub_ = nh_.advertise("sub_goal", 1); + closet_robot_goal_pub_ = nh_.advertise("closet_robot_goal", 1); + transformed_plan_pub_ = nh_.advertise("transformed_plan", 1); + compute_plan_pub_ = nh_.advertise("compute_plan", 1); + + std::vector footprint = costmap_ros_? costmap_ros_->getRobotFootprint() : std::vector(); + if (footprint.size() > 1) + { + double min_length = 1e10; + double max_length = 0.0; + for (size_t i = 0; i < footprint.size(); ++i) + { + const geometry_msgs::Point &p1 = footprint[i]; + const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; + if (len > max_length) + { + max_length = len; + } + if (len < min_length) + { + min_length = len; + } + } + this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; + this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; + } + + // kalman + last_actuator_update_ = ros::Time::now(); + n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] + m_ = 2; // measurements: x, y, theta + double dt = control_duration_; + + // Khởi tạo ma trận + A = Eigen::MatrixXd::Identity(n_, n_); + C = Eigen::MatrixXd::Zero(m_, n_); + Q = Eigen::MatrixXd::Zero(n_, n_); + R = Eigen::MatrixXd::Identity(m_, m_); + P = Eigen::MatrixXd::Identity(n_, n_); + + for (int i = 0; i < n_; i += 3) + { + A(i, i + 1) = dt; + A(i, i + 2) = 0.5 * dt * dt; + A(i + 1, i + 2) = dt; + } + + C(0, 0) = 1; + C(1, 3) = 1; + Q(2, 2) = 0.1; + Q(5, 5) = 0.6; + + R(0, 0) = 0.1; + R(1, 1) = 0.2; + + P(3, 3) = 0.4; + P(4, 4) = 0.4; + P(5, 5) = 0.4; + + kf_ = boost::make_shared(dt, A, C, Q, R, P); + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(ros::Time::now().toSec(), x0); + + x_direction_ = y_direction_ = theta_direction_ = 0; + initialized_ = true; + ROS_INFO("GoStraight Initialized successfully"); + } +} + +mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( + const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + mkt_msgs::Trajectory2D result; + result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; + if (!traj_) + return result; + if (compute_plan_.poses.size() < 2) + { + ROS_WARN_THROTTLE(2.0, "Local compute plan is available"); + return result; + } + + ros::Time current_time = ros::Time::now(); + double dt = (current_time - last_actuator_update_).toSec(); + last_actuator_update_ = current_time; + + nav_2d_msgs::Twist2D drive_cmd; + double sign_x = x_direction_; + + nav_2d_msgs::Twist2D twist; + traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom + ros::Rate r(50); + while (ros::ok() && traj_->hasMoreTwists()) + { + twist = traj_->nextTwist(); + // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta); + r.sleep(); + } + + drive_cmd.x = sqrt(twist.x * twist.x); + nav_2d_msgs::Path2D transformed_plan = transform_plan_; + if (transformed_plan.poses.empty()) + { + ROS_WARN("Transformed plan is empty. Cannot determine a local plan."); + return result; + } + + if (enable_publish_) + transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan)); + + // Dynamically adjust look ahead distance based on the speed + double lookahead_dist = this->getLookAheadDistance(twist); + // Return false if the transformed global plan is empty + geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose; + lookahead_dist += fabs(front.y); + + // Get lookahead point and publish for visualization + auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); + + if (enable_publish_) + carrot_pub_.publish(this->createCarrotMsg(carrot_pose)); + + // Carrot distance squared + const double carrot_dist2 = + (carrot_pose.pose.x * carrot_pose.pose.x) + + (carrot_pose.pose.y * carrot_pose.pose.y); + + // Find curvature of circle (k = 1 / R) + double curvature = 0.0; + if (carrot_dist2 > 2e-2) + { + curvature = 2.0 * carrot_pose.pose.y / carrot_dist2; + } + + // Constrain linear velocity + this->applyConstraints( + 0.0, lookahead_dist, curvature, twist, + this->costAtPose(pose.pose.x, pose.pose.y), + drive_cmd.x, sign_x); + + + if (std::hypot(carrot_pose.pose.x, carrot_pose.pose.y) > min_journey_squared_) + { + nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); + nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; + double dx = end.pose.x - start.pose.x; + double dy = end.pose.y - start.pose.y; + drive_cmd.theta = atan2(dy, dx); + + //[-π/2, π/2] + if (drive_cmd.theta > M_PI / 2) + drive_cmd.theta -= M_PI; + else if (drive_cmd.theta < -M_PI / 2) + drive_cmd.theta += M_PI; + + drive_cmd.theta = std::clamp(drive_cmd.theta, -min_vel_theta_, min_vel_theta_); + } + else + drive_cmd.theta = 0.0; + + // populate and return message + if (fabs(carrot_pose.pose.x) > 1e-9 || carrot_pose.pose.y > 1e-9) + drive_cmd.x *= fabs(cos(std::atan2(carrot_pose.pose.y, carrot_pose.pose.x))); + + double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + const double vel_x_reduce = std::min(fabs(v_max), 0.3); + double journey_plan = compute_plan_.poses.empty() ? 0 : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1); + const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; + const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; + double d_reduce = std::clamp(journey_plan, min_S, max_S); + double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); + double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); + double v_min = + journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; + v_min *= sign_x; + double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); + double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); + journey_plan += max_path_distance_; + drive_cmd.x = journey_plan >= d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * decel_factor + v_min; + + // drive_cmd.x = journey_plan > d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * sin((journey_plan / d_reduce) * (M_PI / 2)) + v_min; + + Eigen::VectorXd y(2); + y << drive_cmd.x, drive_cmd.theta; + + // Cập nhật lại A nếu dt thay đổi + for (int i = 0; i < n_; ++i) + for (int j = 0; j < n_; ++j) + A(i, j) = (i == j ? 1.0 : 0.0); + for (int i = 0; i < n_; i += 3) + { + A(i, i + 1) = dt; + A(i, i + 2) = 0.5 * dt * dt; + A(i + 1, i + 2) = dt; + } + kf_->update(y, dt, A); + + if (drive_cmd.x != v_max) + { + double min = drive_cmd.x < v_max ? drive_cmd.x : v_max; + double max = drive_cmd.x > v_max ? drive_cmd.x : v_max; + drive_cmd.x = std::clamp(kf_->state()[0], min, max); + } + else + { + drive_cmd.x = std::clamp(kf_->state()[0], v_max, v_max); + } + + drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x); + drive_cmd.theta = kf_->state()[3]; + result.velocity = drive_cmd; + return result; +} + +PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::GoStraight, score_algorithm::ScoreAlgorithm) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp new file mode 100644 index 0000000..9cad56e --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -0,0 +1,1587 @@ +#include +// other +#include +#define LIMIT_VEL_THETA 0.33 + +void mkt_algorithm::diff::PredictiveTrajectory::initialize( + ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) +{ + if (!initialized_) + { + nh_ = ros::NodeHandle("~/"); + nh_priv_ = ros::NodeHandle("~/" + name); + name_ = name; + tf_ = tf; + traj_ = traj; + costmap_ros_ = costmap_ros; + this->getParams(); + // this->initDynamicReconfigure(nh_priv_); + nh_.param("publish_topic", enable_publish_, false); + nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); + + std::vector y_vels = {}; + footprint_spec_ = costmap_ros_->getRobotFootprint(); + world_model_ = new base_local_planner::CostmapModel(*costmap_ros->getCostmap()); + + this->config_.acc_lim_x = fabs(acc_lim_x_); + this->config_.acc_lim_y = fabs(acc_lim_y_); + this->config_.acc_lim_theta = fabs(acc_lim_theta_); + + this->config_.max_vel_x = max_vel_x_; + this->config_.min_vel_x = fabs(min_vel_x_); + + this->config_.max_vel_theta = max_vel_theta_; + this->config_.min_vel_theta = (-1.0) * max_vel_theta_; + this->config_.min_in_place_vel_theta = 0.4; + + this->config_.sim_time = 1.5; + this->config_.sim_granularity = 0.025; + this->config_.vx_samples = 10; + this->config_.vtheta_samples = 40; + this->config_.path_distance_bias = 0.6; + this->config_.goal_distance_bias = 0.4; + this->config_.occdist_scale = 0.2; + this->config_.heading_lookahead = 1.0; + this->config_.oscillation_reset_dist = 0.05; + this->config_.escape_reset_dist = 0.1; + this->config_.escape_reset_theta = 1.570796; + this->config_.escape_vel = -0.1; + this->config_.holonomic_robot = false; + this->config_.heading_scoring_timestep = 0.1; + this->config_.dwa = true; + this->config_.simple_attractor = true; + this->config_.heading_scoring = false; + this->config_.angular_sim_granularity = 0.025; + bool meter_scoring = false; + double stop_time_buffer = 0.2; + + tc_ = new base_local_planner::TrajectoryPlanner( + *world_model_, *costmap_ros->getCostmap(), footprint_spec_, + this->config_.acc_lim_x, this->config_.acc_lim_y, this->config_.acc_lim_theta, + this->config_.sim_time, this->config_.sim_granularity, + this->config_.vx_samples, this->config_.vtheta_samples, + this->config_.path_distance_bias, this->config_.goal_distance_bias, this->config_.occdist_scale, + this->config_.heading_lookahead, this->config_.oscillation_reset_dist, + this->config_.escape_reset_dist, this->config_.escape_reset_theta, + this->config_.holonomic_robot, + this->config_.max_vel_x, this->config_.min_vel_x, + this->config_.max_vel_theta, this->config_.min_vel_theta, this->config_.min_in_place_vel_theta, + this->config_.escape_vel, + this->config_.dwa, this->config_.heading_scoring, this->config_.heading_scoring_timestep, meter_scoring, this->config_.simple_attractor, + y_vels, stop_time_buffer, control_duration_, this->config_.angular_sim_granularity); + + map_viz_.initialize(name, + costmap_ros_->getGlobalFrameID(), + [this](int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) + { + return tc_->getCellCosts(cx, cy, path_cost, goal_cost, occ_cost, total_cost); + }); + + carrot_pub_ = nh_.advertise("lookahead_point", 1); + reached_intermediate_goals_pub_ = nh_.advertise("reached_intermediate_goals", 1); + current_goal_pub_ = nh_.advertise("sub_goal", 1); + closet_robot_goal_pub_ = nh_.advertise("closet_robot_goal", 1); + transformed_plan_pub_ = nh_.advertise("transformed_plan", 1); + compute_plan_pub_ = nh_.advertise("compute_plan", 1); + cost_right_goals_pub_ = nh_.advertise("cost_right_goals", 1); + cost_left_goals_pub_ = nh_.advertise("cost_left_goals", 1); + drive_pub_ = nh_.advertise("cmd_vel_raw", 1); + + std::vector footprint = costmap_ros_ ? costmap_ros_->getRobotFootprint() : std::vector(); + if (footprint.size() > 1) + { + double min_length = 1e10; + double max_length = 0.0; + for (size_t i = 0; i < footprint.size(); ++i) + { + const geometry_msgs::Point &p1 = footprint[i]; + const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; + if (len > max_length) + { + max_length = len; + } + if (len < min_length) + { + min_length = len; + } + } + this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; + this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; + } + + // kalman + last_actuator_update_ = ros::Time::now(); + n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] + m_ = 2; // measurements: x, y, theta + double dt = control_duration_; + + // Khởi tạo ma trận + A = Eigen::MatrixXd::Identity(n_, n_); + C = Eigen::MatrixXd::Zero(m_, n_); + Q = Eigen::MatrixXd::Zero(n_, n_); + R = Eigen::MatrixXd::Identity(m_, m_); + P = Eigen::MatrixXd::Identity(n_, n_); + + for (int i = 0; i < n_; i += 3) + { + A(i, i + 1) = dt; + A(i, i + 2) = 0.5 * dt * dt; + A(i + 1, i + 2) = dt; + } + + C(0, 0) = 1; + C(1, 3) = 1; + Q(2, 2) = 0.1; + Q(5, 5) = 0.6; + + R(0, 0) = 0.1; + R(1, 1) = 0.2; + + P(3, 3) = 0.4; + P(4, 4) = 0.4; + P(5, 5) = 0.4; + + kf_ = boost::make_shared(dt, A, C, Q, R, P); + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(ros::Time::now().toSec(), x0); + + L_.header.frame_id = R_.header.frame_id = costmap_ros_->getGlobalFrameID(); // hoặc frame phù hợp + L_.ns = "left_cost_points"; + R_.ns = "right_cost_points"; + L_.id = 0; + R_.id = 1; + L_.type = R_.type = visualization_msgs::Marker::POINTS; + L_.action = R_.action = visualization_msgs::Marker::ADD; + L_.scale.x = L_.scale.y = R_.scale.x = R_.scale.y = 0.05; // kích thước điểm + // màu: L = xanh lá, R = đỏ + L_.color.r = 0.0; + L_.color.g = 1.0; + L_.color.b = 0.0; + L_.color.a = 1.0; + R_.color.r = 1.0; + R_.color.g = 0.0; + R_.color.b = 0.0; + R_.color.a = 1.0; + + x_direction_ = y_direction_ = theta_direction_ = 0; + this->initialized_ = true; + ROS_INFO("PredictiveTrajectory Initialized successfully"); + } +} + +mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() +{ + if (ddr_) + ddr_.reset(); + + if (tc_ != NULL) + delete tc_; + + if (world_model_ != NULL) + delete world_model_; +} + +void mkt_algorithm::diff::PredictiveTrajectory::getParams() +{ + robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); + nh_priv_.param("avoid_obstacles", avoid_obstacles_, false); + nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); + nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); + nh_priv_.param("index_samples", index_samples_, 0); + nh_priv_.param("follow_step_path", follow_step_path_, false); + + nh_priv_.param("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false); + nh_priv_.param("lookahead_dist", lookahead_dist_, 0.25); + nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.3); + nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 0.9); + nh_priv_.param("lookahead_time", lookahead_time_, 1.5); + nh_priv_.param("min_journey_squared", min_journey_squared_, std::numeric_limits::infinity()); + nh_priv_.param("max_journey_squared", max_journey_squared_, std::numeric_limits::infinity()); + + nh_priv_.param("use_rotate_to_heading", use_rotate_to_heading_, false); + nh_priv_.param("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785); + + nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.0); + nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.0); + if (trans_stopped_velocity_ <= min_approach_linear_velocity_ || trans_stopped_velocity_ - min_approach_linear_velocity_ < 0.02) + trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.05; + + // Regulated linear velocity scaling + nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); + nh_priv_.param("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9); + nh_priv_.param("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25); + + // Inflation cost scaling (Limit velocity by proximity to obstacles) + nh_priv_.param("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true); + nh_priv_.param("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0); + nh_priv_.param("cost_scaling_dist", cost_scaling_dist_, 0.6); + nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); + if (inflation_cost_scaling_factor_ <= 0.0) + { + ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + use_cost_regulated_linear_velocity_scaling_ = false; + } + double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); + control_duration_ = 1.0 / control_frequency; + window_size_ = (size_t)(control_frequency * 3.0); + + if (traj_) + { + traj_.get()->getNodeHandle().param("max_vel_x", max_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_x", min_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_x", acc_lim_x_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_x", decel_lim_x_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_y", max_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_y", min_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_y", acc_lim_y_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_y", decel_lim_y_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_theta", max_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_theta", min_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_theta", acc_lim_theta_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_theta", decel_lim_theta_, 0.0); + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::initDynamicReconfigure(const ros::NodeHandle &nh) +{ + // Ddynamic Reconfigure + ddr_ = std::make_shared(nh); + ddr_->registerVariable("avoid_obstacles", &this->avoid_obstacles_, "", true); + ddr_->registerVariable("lookahead_time", &this->lookahead_time_, "", 0.0, 20.0); + ddr_->registerVariable("lookahead_dist", &this->lookahead_dist_, "", 0.0, 20.0); + + ddr_->registerVariable("use_velocity_scaled_lookahead_dist", &this->use_velocity_scaled_lookahead_dist_); + ddr_->registerVariable("min_lookahead_dist", &this->min_lookahead_dist_, "", 0.0, 5.0); + ddr_->registerVariable("max_lookahead_dist", &this->max_lookahead_dist_, "", 0.0, 10.0); + ddr_->registerVariable("min_journey_squared", &this->min_journey_squared_, "", 0.0, 1.0); + ddr_->registerVariable("max_journey_squared", &this->max_journey_squared_, "", 0.0, 1.0); + // Rotate to heading param + ddr_->registerVariable("use_rotate_to_heading", &this->use_rotate_to_heading_); + ddr_->registerVariable("rotate_to_heading_min_angle", &this->rotate_to_heading_min_angle_, "", 0.0, 15.0); + + // Speed + ddr_->registerVariable("min_approach_linear_velocity", &this->min_approach_linear_velocity_, "", 0.0, 10.0); + + // Regulated linear velocity scaling + ddr_->registerVariable("use_regulated_linear_velocity_scaling", &this->use_regulated_linear_velocity_scaling_); + ddr_->registerVariable("regulated_linear_scaling_min_radius", &this->regulated_linear_scaling_min_radius_, "", 0.0, 5.0); + ddr_->registerVariable("regulated_linear_scaling_min_speed", &this->regulated_linear_scaling_min_speed_, "", 0.0, 5.0); + + // Inflation cost scaling (Limit velocity by proximity to obstacles) + ddr_->registerVariable("use_cost_regulated_linear_velocity_scaling", &this->use_cost_regulated_linear_velocity_scaling_); + ddr_->registerVariable("inflation_cost_scaling_factor", &this->inflation_cost_scaling_factor_, "", 0.0, 10.0); + ddr_->registerVariable("cost_scaling_dist", &this->cost_scaling_dist_, "", 0.0, 10.0); + ddr_->registerVariable("cost_scaling_gain", &this->cost_scaling_gain_, "", 0.0, 10.0); + + ddr_->publishServicesTopics(); +} + +void mkt_algorithm::diff::PredictiveTrajectory::reset() +{ + if (this->initialized_) + { + ROS_INFO("PredictiveTrajectory is reset"); + reached_intermediate_goals_.clear(); + start_index_saved_vt_.clear(); + this->clear(); + x_direction_ = y_direction_ = theta_direction_ = 0; + this->follow_step_path_ = false; + this->nav_stop_ = false; + last_actuator_update_ = ros::Time::now(); + prevous_drive_cmd_ = nav_2d_msgs::Twist2D(); + + if (kf_) + { + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(ros::Time::now().toSec(), x0); + } + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::stop() +{ + this->nav_stop_ = true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::resume() +{ + if (this->initialized_) + { + if(!this->nav_stop_) + return; + prevous_drive_cmd_ = nav_2d_msgs::Twist2D(); + last_actuator_update_ = ros::Time::now(); + + if (kf_) + { + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(ros::Time::now().toSec(), x0); + } + this->nav_stop_ = false; + } +} + +bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, + const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, + double &x_direction, double &y_direction, double &theta_direction) +{ + if (!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + std::vector footprint = costmap_ros_ ? costmap_ros_->getRobotFootprint() : std::vector(); + if (footprint.size() > 1) + { + double min_length = 1e10; + double max_length = 0.0; + for (size_t i = 0; i < footprint.size(); ++i) + { + const geometry_msgs::Point &p1 = footprint[i]; + const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; + if (len > max_length) + { + max_length = len; + } + if (len < min_length) + { + min_length = len; + } + } + this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; + this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; + } + + if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2) + { + ROS_ERROR("The Local plan is empty or less than 1 points %d", (unsigned int)global_plan.poses.size()); + return false; + } + this->getParams(); + if (avoid_obstacles_) + map_viz_.publishCostCloud(costmap_ros_->getCostmap()); + + frame_id_path_ = global_plan.header.frame_id; + goal_ = goal; + global_plan_ = global_plan; + + // prune global plan to cut off parts of the past (spatially before the robot) + if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0)) + { + ROS_ERROR("pruneGlobalPlan Failed"); + return false; + } + + double S = std::numeric_limits::infinity(); + S = std::max(costmap_ros_->getCostmap()->getSizeInCellsX() * costmap_ros_->getCostmap()->getResolution() / 2.0, + costmap_ros_->getCostmap()->getSizeInCellsY() * costmap_ros_->getCostmap()->getResolution() / 2.0); + const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_; + S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S); + compute_plan_.poses.clear(); + + if ((unsigned int)global_plan_.poses.size() == 2) + { + double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x; + double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y; + if (hypot(dx, dy) <= 2.0 * costmap_ros_->getCostmap()->getResolution()) + { + compute_plan_ = global_plan_; + } + else + { + ROS_ERROR("The Local Plan has 2 points and hypot between more than is %.3f m", costmap_ros_->getCostmap()->getResolution()); + return false; + } + } + else + { + unsigned int start_index, goal_index; + if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_)) + { + ROS_ERROR("computePlanCommand is Failed"); + return false; + } + } + + if (enable_publish_) + compute_plan_pub_.publish(nav_2d_utils::pathToPath(compute_plan_)); + + double lookahead_dist = this->getLookAheadDistance(velocity); + transform_plan_.poses.clear(); + if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_ros_, robot_base_frame_, lookahead_dist, transform_plan_)) + { + ROS_WARN("Could not transform the global plan to the frame of the controller"); + return false; + } + // else + // { + // ROS_INFO_THROTTLE(0.5, "Transform plan journey: %f %f %f", journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_); + // } + + x_direction = x_direction_; + y_direction = y_direction_ = 0; + theta_direction = theta_direction_; + + if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq) + { + const geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose; + int index; + for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--) + { + geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose; + if (nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_) + break; + } + const geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose; + if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + { + const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x); + const double dir_path = cos(fabs(path_angle - p2.theta)); + if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) + x_direction = dir_path > 0 ? FORWARD : BACKWARD; + } + } + else + { + try + { + auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_); + auto prev_carrot_pose_it = transform_plan_.poses.begin(); + double distance_it = 0; + for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it) + { + double dx = it->pose.x - carrot_pose_it->pose.x; + double dy = it->pose.x - carrot_pose_it->pose.y; + distance_it += std::hypot(dx, dy); + if (distance_it > costmap_ros_->getCostmap()->getResolution()) + { + prev_carrot_pose_it = it; + break; + } + } + + geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_ros_->getCostmap()->getResolution() + ? nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose) + : nav_2d_utils::pose2DToPose(geometry_msgs::Pose2D()); + + geometry_msgs::Pose back = nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose); + + teb_local_planner::PoseSE2 start_pose(front); + teb_local_planner::PoseSE2 goal_pose(back); + const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec()); + + if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) + x_direction = dir_path > 0 ? FORWARD : BACKWARD; + } + catch (std::exception &e) + { + ROS_ERROR("getLookAheadPoint throw an exception: %s", e.what()); + return false; + } + } + + x_direction_ = x_direction; + y_direction_ = y_direction; + theta_direction_ = theta_direction; + return true; +} + +mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( + const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + mkt_msgs::Trajectory2D result; + result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; + if (!traj_) + return result; + if (compute_plan_.poses.size() < 2) + { + ROS_WARN("Local compute plan is available"); + return result; + } + + ros::Time current_time = ros::Time::now(); + double dt = (current_time - last_actuator_update_).toSec(); + last_actuator_update_ = current_time; + + nav_2d_msgs::Twist2D drive_cmd; + double sign_x = sign(x_direction_); + nav_2d_msgs::Twist2D twist; + traj_->startNewIteration(velocity); + while (ros::ok() && traj_->hasMoreTwists()) + { + twist = traj_->nextTwist(); + } + drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); + double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + + if (avoid_obstacles_) + { + if ((cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE) || + (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE)) + { + nav_2d_msgs::Twist2D cmd; + cmd.x = 0.2; + moveWithAccLimits(velocity, cmd, drive_cmd); + config_.max_vel_x = drive_cmd.x; + config_.sim_time = 1.9; + config_.path_distance_bias = 0.7; + config_.goal_distance_bias = 0.6; + config_.occdist_scale = 0.05; + config_.dwa = true; + } + else if (cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE || + cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE) + { + nav_2d_msgs::Twist2D cmd; + cmd.x = 0.3; + moveWithAccLimits(velocity, cmd, drive_cmd); + config_.max_vel_x = drive_cmd.x; + config_.sim_time = 1.9; + config_.path_distance_bias = 0.7; + config_.goal_distance_bias = 0.6; + config_.occdist_scale = 0.12; + config_.dwa = false; + } + else + { + config_.max_vel_x = std::max(drive_cmd.x, 0.2); + config_.sim_time = 1.5; + config_.path_distance_bias = 0.8; + config_.goal_distance_bias = 0.6; + config_.occdist_scale = 0.01; + config_.dwa = false; + } + tc_->reconfigure(config_); + this->publishMarkers(pose); + } + + nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; + if (transformed_plan.poses.empty()) + { + ROS_WARN("Transformed plan is empty. Cannot determine a localglobal_plan."); + return result; + } + if (enable_publish_) + transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan)); + + double lookahead_dist = getLookAheadDistance(velocity); + double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y); + // lookahead_dist = std::clamp(lookahead_dist - tolerance * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_); + + if (transformed_plan.poses.empty()) + { + ROS_WARN("Transformed plan is empty after compute lookahead point"); + return result; + } + auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); + + if (enable_publish_) + carrot_pub_.publish(nav_2d_utils::pose2DToPoseStamped(carrot_pose.pose, carrot_pose.header.frame_id, ros::Time::now())); + + bool allow_rotate = false; + nh_priv_.param("allow_rotate", allow_rotate, false); + + // double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y); + geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose; + const double distance_allow_rotate = min_journey_squared_; + const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y); + const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1); + if (avoid_obstacles_) + allow_rotate = journey_plan >= distance_allow_rotate && + fabs(front.y) <= 0.2 && + (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_ros_->getCostmap()->getResolution()); + else + allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; + + double angle_to_heading; + if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) + { + if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_)) + { + tc_->updatePlan(nav_2d_utils::pathToPath(compute_plan_).poses, true); + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; + } + else + { + rotateToHeading(angle_to_heading, velocity, drive_cmd); + } + } + else + { + bool narrow_road = (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE); + if (!avoid_obstacles_ || sign_x < 0 || journey_plan < max_journey_squared_ || narrow_road) + { + const double vel_x_reduce = std::min(fabs(v_max), 0.3); + double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; + carrot_dist2 = std::max(carrot_dist2, 0.05); + double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + + const auto &plan_back_pose = compute_plan_.poses.back(); + double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y); + post_cost = std::max(post_cost, center_cost_); + this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); + + const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; + const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; + double d_reduce = std::clamp(journey_plan, min_S, max_S); + double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); + double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); + double v_min = + journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; + v_min *= sign_x; + + double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); + double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); + double vel_reduce = sign_x > 0 + ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min) + : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min); + drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce; + + double v_theta = drive_cmd.x * curvature; + double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_)) + { + carrot_dist2 *= 0.6; + curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + v_theta = drive_cmd.x * curvature; + } + if (fabs(v_theta) > LIMIT_VEL_THETA) + { + nav_2d_msgs::Twist2D cmd_vel, cmd_result; + cmd_vel.x = sign_x > 0 + ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1)) + : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1)); + cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5); + this->moveWithAccLimits(velocity, cmd_vel, cmd_result); + drive_cmd.x = std::copysign(cmd_result.x, sign_x); + v_theta = drive_cmd.x * curvature; + } + + if (journey_plan < min_journey_squared_) + { + if (transform_plan_.poses.size() > 2) + { + nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); + nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; + double dx = end.pose.x - start.pose.x; + double dy = end.pose.y - start.pose.y; + v_theta = atan2(dy, dx); + if (v_theta > M_PI_2) + v_theta -= M_PI; + else if (v_theta < -M_PI_2) + v_theta += M_PI; + // v_theta *= 0.5; + v_theta = std::clamp(v_theta, -0.02, 0.02); + } + else + v_theta = 0.0; + } + double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8; + double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; + double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; + drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); + + if (this->nav_stop_) + { + if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) + { + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; + } + else + drive_cmd = {}; + result.velocity = drive_cmd; + return result; + } + } + else + { + geometry_msgs::PoseStamped traj_cmd, robot_vel; + robot_vel.pose.position.x = velocity.x; + robot_vel.pose.position.y = velocity.y; + tf2::Quaternion q; + + q.setRPY(0, 0, velocity.theta); + tf2::convert(q, robot_vel.pose.orientation); + tc_->updatePlan(nav_2d_utils::pathToPath(compute_plan_).poses, true); + auto path = tc_->findBestPath(nav_2d_utils::pose2DToPoseStamped(pose), robot_vel, traj_cmd); + + if (path.cost_ < 0) + { + ROS_DEBUG_NAMED("trajectory_planner_ros", + "The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories."); + + throw nav_core2::PlannerTFException("The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories."); + } + + if (path.cost_ < 0 || nav_stop_) + { + if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) + { + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; + } + else + drive_cmd = {}; + result.velocity = drive_cmd; + return result; + } + else + { + drive_cmd.x = std::max(drive_cmd.x, min_approach_linear_velocity_); + drive_cmd.x = std::clamp(traj_cmd.pose.position.x, -fabs(drive_cmd.x), fabs(drive_cmd.x)); + drive_cmd.y = traj_cmd.pose.position.y; + drive_cmd.theta = tf2::getYaw(traj_cmd.pose.orientation); + + if ((cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE || + cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE) && + fabs(traj_cmd.pose.position.x) < 0.02 && fabs(drive_cmd.theta) > 0.1) + { + nav_2d_msgs::Twist2D cmd, cmd_result; + cmd.x = sign_x * min_approach_linear_velocity_; + moveWithAccLimits(velocity, cmd, cmd_result); + + if (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE) + drive_cmd.theta = 0.0; + + else if (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE) + drive_cmd.theta = -min_vel_theta_ * sign_x; + + else + drive_cmd.theta = min_vel_theta_ * sign_x; + drive_cmd.x = sign_x * cmd_result.x; + } + + for (unsigned int i = 0; i < path.getPointsSize(); ++i) + { + double p_x, p_y, p_th; + path.getPoint(i, p_x, p_y, p_th); + geometry_msgs::Pose2D pose; + pose.x = p_x; + pose.y = p_y; + pose.theta = p_th; + result.poses.push_back(pose); + } + } + } + + Eigen::VectorXd y(2); + y << drive_cmd.x, drive_cmd.theta; + + // Cập nhật lại A nếu dt thay đổi + for (int i = 0; i < n_; ++i) + for (int j = 0; j < n_; ++j) + A(i, j) = (i == j ? 1.0 : 0.0); + for (int i = 0; i < n_; i += 3) + { + A(i, i + 1) = dt; + A(i, i + 2) = 0.5 * dt * dt; + A(i + 1, i + 2) = dt; + } + kf_->update(y, dt, A); + + // Check if Kalman filter's estimated velocity exceeds v_max + if (avoid_obstacles_ && (cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE || + cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE || + cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE || + cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE)) + { + drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0]; + } + else + { + drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); + } + + drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x); + drive_cmd.theta = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA); + } + + result.velocity = drive_cmd; + prevous_drive_cmd_ = drive_cmd; + return result; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( + const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &carrot_pose, const nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x) +{ + bool curvature = false; + double path_angle = 0; + if (!global_plan.poses.empty()) + { + if ((unsigned)global_plan.poses.size() > 2) + { + const double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; + const double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; + if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + { + double start_angle = atan2(start_direction_y, start_direction_x); + for (unsigned int i = 1; i < (unsigned)global_plan.poses.size(); i++) + { + const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; + const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; + if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + { + double current_angle = atan2(current_direction_y, current_direction_x); + if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > 1e-9) + { + curvature = true; + break; + } + path_angle = current_angle; + } + } + } + } + } + // ROS_INFO_THROTTLE(0.1,"path_angle %.3f %.3f", path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x)); + // Whether we should rotate robot to rough path heading + angle_to_path = curvature ? path_angle : std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + angle_to_path = sign_x < 0 + ? angles::normalize_angle(M_PI + angle_to_path) + : angles::normalize_angle(angle_to_path); + + double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y); + // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + double heading_rotate = rotate_to_heading_min_angle_; + bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_); + heading_rotate = is_stopped + ? rotate_to_heading_min_angle_ + heading_linear + : std::clamp(heading_rotate + heading_linear, 0.3, M_PI_2); + + bool result = use_rotate_to_heading_ && (is_stopped || sign(angle_to_path) * sign_x < 0) && fabs(angle_to_path) > heading_rotate; + + // if (result) + // { + // ROS_WARN_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f", + // velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_); + // ROS_WARN_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n", + // is_stopped, path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x), angle_to_path, heading_rotate, sign_x); + // } + // else + // { + // ROS_INFO_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f", + // velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_); + // ROS_INFO_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n", + // is_stopped, path_angle,std::atan2(carrot_pose.pose.y, carrot_pose.pose.x) ,angle_to_path, heading_rotate, sign_x); + // } + + return result; +} + +void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &angle_to_path, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel) +{ + const double dt = control_duration_; + const double theta_direction = angle_to_path > 0.0 ? 1.0 : -1.0; + double vel_yaw = theta_direction * fabs(velocity.theta); + double ang_diff = angle_to_path; + double max_vel_theta = traj_ ? fabs(traj_->getTwistAngular(true).z) : 0.3; + + double angular_decel_zone = 0.3; // rad - khoảng cách bắt đầu giảm tốc + nh_priv_.param("angular_decel_zone", angular_decel_zone, 0.5); + angular_decel_zone += std::clamp( + fabs(max_vel_theta) + (M_PI_2 - fabs(atan2(min_path_distance_, max_path_distance_))), 0.4, M_PI_2); + double zone_begin = std::max(angular_decel_zone * 0.2, 0.2); + double cosine_begin_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / zone_begin))); + + double reduce_vel_theta = std::min(max_vel_theta, 0.25 + min_vel_theta_); + reduce_vel_theta = fabs(ang_diff) > zone_begin ? reduce_vel_theta : (reduce_vel_theta - min_vel_theta_) * cosine_begin_factor + min_vel_theta_; + + // === ANGULAR DECELERATION ZONE === + double target_angular_speed = max_vel_theta; + if (fabs(ang_diff) < angular_decel_zone) + { + // ROS_WARN_THROTTLE(0.2,"%f %f %f %f", ang_diff, angular_decel_zone, zone_begin, max_vel_theta); + double cosine_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / angular_decel_zone))); + double cosine_speed = max_vel_theta * cosine_factor; + + // Choose deceleration profile + target_angular_speed = cosine_speed; // Smoothest option + + // Ensure minimum speed to avoid stalling + target_angular_speed = std::max(target_angular_speed, reduce_vel_theta); + } + // else + // ROS_INFO_THROTTLE(1,"%f %f %f %f", ang_diff, angular_decel_zone, max_vel_theta, reduce_vel_theta); + + // Apply direction + double v_theta_desired = std::copysign(target_angular_speed, ang_diff); + + // === ACCELERATION LIMITS === + double max_acc_vel = vel_yaw + fabs(acc_lim_theta_) * dt; + double min_acc_vel = vel_yaw - fabs(acc_lim_theta_) * dt; + double v_theta_samp = std::clamp(v_theta_desired, min_acc_vel, max_acc_vel); + + // === STOPPING CONSTRAINT === + double max_speed_to_stop = sqrt(2.0 * fabs(decel_lim_theta_) * fabs(ang_diff)); + v_theta_samp = std::copysign( + std::min(max_speed_to_stop, fabs(v_theta_samp)), ang_diff); + + // === FINAL LIMITS === + v_theta_samp = theta_direction > 0 + ? std::clamp(v_theta_samp, min_vel_theta_, max_vel_theta) + : std::clamp(v_theta_samp, -max_vel_theta, -min_vel_theta_); + + cmd_vel.x = 0.0; + cmd_vel.y = 0.0; + cmd_vel.theta = v_theta_samp; +} + +double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const nav_2d_msgs::Twist2D &velocity) +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the static look ahead distance + double lookahead_dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) + { + lookahead_dist = fabs(velocity.x) * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } + + return lookahead_dist; +} + +std::vector::iterator +mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, nav_2d_msgs::Path2D global_plan) +{ + if (global_plan.poses.empty()) + { + throw nav_core2::PlannerTFException("The global plan passed to the local planner is empty."); + } + + if ((unsigned)global_plan.poses.size() < 2) + { + auto goal_pose_it = std::prev(global_plan.poses.end()); + return goal_pose_it; + } + unsigned int goal_index = (unsigned)global_plan.poses.size() - 1; + + double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; + double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; + double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; + double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; + + // make sure that atan2 is defined + double start_angle = atan2(start_direction_y, start_direction_x); + double end_angle = atan2(end_direction_y, end_direction_x); + double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); + + for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++) + { + if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + { + const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; + const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.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_index = i; + if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_)) + continue; + + if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold) + { + goal_index = i; + break; + } + } + } + } + + // Find the first pose which is at a distance greater than the lookahead distance + auto goal_pose_it = std::find_if( + global_plan.poses.begin(), global_plan.poses.begin() + goal_index, + [&](const auto &ps) + { + return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; + }); + + // If the number of poses is not far enough, take the last pose + if (goal_pose_it == global_plan.poses.end()) + { + goal_pose_it = std::prev(global_plan.poses.end()); + } + return goal_pose_it; +} + +geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const nav_2d_msgs::Pose2DStamped &carrot_pose) +{ + geometry_msgs::PointStamped carrot_msg; + carrot_msg.header = carrot_pose.header; + carrot_msg.point.x = carrot_pose.pose.x; + carrot_msg.point.y = carrot_pose.pose.y; + carrot_msg.point.z = 0.5; // publish right over map to stand out + return carrot_msg; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose, nav_2d_msgs::Path2D &global_plan, double dist_behind_robot) +{ + if (global_plan.poses.empty()) + return false; + if (tf == nullptr) + return false; + try + { + // let's get the pose of the robot in the frame of the plan + nav_2d_msgs::Pose2DStamped robot; + if (!nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot)) + { + throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); + } + + double dist_thresh_sq = dist_behind_robot * dist_behind_robot; + + // iterate plan until a pose close the robot is found + std::vector::iterator it = global_plan.poses.begin(); + std::vector::iterator erase_end = it; + while (it != global_plan.poses.end()) + { + double dx = robot.pose.x - it->pose.x; + double dy = robot.pose.y - it->pose.y; + double dist_sq = dx * dx + dy * dy; + if (dist_sq < dist_thresh_sq) + { + erase_end = it; + break; + } + ++it; + } + if (erase_end == global_plan.poses.end()) + return false; + + if (erase_end != global_plan.poses.begin()) + global_plan.poses.erase(global_plan.poses.begin(), erase_end); + } + catch (const tf::TransformException &ex) + { + ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what()); + return false; + } + return true; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( + TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose, + const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length, + nav_2d_msgs::Path2D &transformed_plan) +{ + // this method is a slightly modified version of base_local_planner/goal_functions.h + transformed_plan.poses.clear(); + transformed_plan.header.stamp = pose.header.stamp; + transformed_plan.header.frame_id = robot_base_frame; + + if (global_plan.poses.empty()) + return false; + + if (tf == nullptr) + return false; + + if (costmap == nullptr || costmap->getCostmap() == nullptr) + return false; + + try + { + if (global_plan.poses.empty()) + { + ROS_ERROR("Received plan with zero length"); + return false; + } + + // let's get the pose of the robot in the frame of the plan + nav_2d_msgs::Pose2DStamped robot_pose; + if (!nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) + { + throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); + } + + // we'll discard points on the plan that are outside the local costmap + double dist_threshold = std::max(costmap->getCostmap()->getSizeInCellsX() * costmap->getCostmap()->getResolution() / 2.0, + costmap->getCostmap()->getSizeInCellsY() * costmap->getCostmap()->getResolution() / 2.0); + + dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are + double sq_dist_threshold = dist_threshold * dist_threshold; + // located on the border of the local costmap + + int i = 0; + double sq_dist = std::numeric_limits::infinity(); + + // we need to loop to a point on the plan that is within a certain distance of the robot + bool robot_reached = false; + for (int j = 0; j < (int)global_plan.poses.size(); ++j) + { + double x_diff = robot_pose.pose.x - global_plan.poses[j].pose.x; + double y_diff = robot_pose.pose.y - global_plan.poses[j].pose.y; + double new_sq_dist = x_diff * x_diff + y_diff * y_diff; + if (robot_reached && new_sq_dist > sq_dist_threshold) + { + break; + } // force stop if we have reached the costmap border + + if (new_sq_dist < sq_dist) // find closest distance + { + sq_dist = new_sq_dist; + i = j; + if (sq_dist < costmap->getCostmap()->getResolution()) // 5 cm to the robot; take the immediate local minima; if it's not the global + robot_reached = true; // minima, probably means that there's a loop in the path, and so we prefer this + } + } + + double plan_length = 0; // check cumulative Euclidean distance along the plan + double x_direction_saved = 0.0; + // now we'll transform until points are outside of our distance threshold + while (i < (int)global_plan.poses.size() && sq_dist <= sq_dist_threshold && + (max_plan_length <= 0 || plan_length <= max_plan_length)) + { + nav_2d_msgs::Pose2DStamped stamped_pose; + stamped_pose.pose = global_plan.poses[i].pose; + stamped_pose.header.frame_id = global_plan.header.frame_id; + + nav_2d_msgs::Pose2DStamped newer_pose; + if (!nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, newer_pose)) + { + ++i; + continue; + } + transformed_plan.poses.push_back(newer_pose); + + double x_diff = robot_pose.pose.x - global_plan.poses[i].pose.x; + double y_diff = robot_pose.pose.y - global_plan.poses[i].pose.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + // Calculate distance to previous pose + if (i > 0 && max_plan_length > 0) + { + geometry_msgs::Pose2D p1 = global_plan.poses[i].pose; + geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose; + plan_length += std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2)); + if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + { + const double egde_angle = atan2(p1.y - p2.y, p1.x - p2.x); + const double target_direction = cos(fabs(egde_angle - p1.theta)); + if (target_direction * x_direction_saved < 0.0) + { + plan_length = 0; + } + x_direction_saved = target_direction; + } + } + ++i; + } + } + catch (tf::LookupException &ex) + { + ROS_ERROR("No Transform available Error: %s\n", ex.what()); + return false; + } + catch (tf::ConnectivityException &ex) + { + ROS_ERROR("Connectivity Error: %s\n", ex.what()); + return false; + } + catch (tf::ExtrapolationException &ex) + { + ROS_ERROR("Extrapolation Error: %s\n", ex.what()); + if (global_plan.poses.size() > 0) + ROS_ERROR("Robot Frame: %s Plan Frame size %d: %s\n", robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); + + return false; + } + return true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( + const nav_2d_msgs::Twist2D &velocity, const nav_2d_msgs::Twist2D &cmd_vel, nav_2d_msgs::Twist2D &cmd_vel_result) +{ + const double dt = control_duration_; + + // --- X axis --- + double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); + double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); + + double vx = cmd_vel.x > 0.0 ? std::min(max_vel_x, std::max(min_vel_x, cmd_vel.x)) : std::max(-max_vel_x, std::min(-min_vel_x, cmd_vel.x)); + + double max_acc_vx = velocity.x + fabs(acc_lim_x_) * dt; + double min_acc_vx = velocity.x - fabs(decel_lim_x_) * dt; + vx = std::clamp(vx, min_acc_vx, max_acc_vx); + + // --- Y axis --- + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double vy = cmd_vel.y > 0.0 ? std::min(max_vel_y, std::max(min_vel_y, cmd_vel.y)) : std::max(-max_vel_y, std::min(-min_vel_y, cmd_vel.y)); + + double max_acc_vy = velocity.y + fabs(acc_lim_y_) * dt; + double min_acc_vy = velocity.y - fabs(decel_lim_y_) * dt; + vy = std::clamp(vy, min_acc_vy, max_acc_vy); + // --- Assign result --- + cmd_vel_result.x = vx; + cmd_vel_result.y = vy; + // cmd_vel_result.theta = vth; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel) +{ + // slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible + // but we'll use a tenth of a second to be consistent with the implementation of the local planner. + const double dt = control_duration_; + double vx = sign(velocity.x) * std::max(0.0, (fabs(velocity.x) - fabs(decel_lim_x_) * dt)); + double vy = sign(velocity.y) * std::max(0.0, (fabs(velocity.y) - fabs(decel_lim_y_) * dt)); + + double vel_yaw = velocity.theta; + double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt)); + + // we do want to check whether or not the command is valid + if (avoid_obstacles_) + { + try + { + if (tc_ != NULL) + { + bool valid_cmd = tc_->checkTrajectory(pose.pose.x, pose.pose.y, pose.pose.theta, velocity.x, velocity.y, vel_yaw, vx, vy, vth); + // if we have a valid command, we'll pass it on, otherwise we'll command all zeros + if (valid_cmd) + { + ROS_DEBUG("Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth); + cmd_vel.x = vx; + cmd_vel.y = vy; + cmd_vel.theta = vth; + return true; + } + } + } + catch (const std::exception &e) + { + ROS_ERROR_STREAM(e.what() << '\n'); + } + } + cmd_vel.x = vx; + cmd_vel.y = vy; + cmd_vel.theta = vth; + return true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( + const double &dist_error, const double &lookahead_dist, + const double &curvature, const nav_2d_msgs::Twist2D &velocity, + const double &pose_cost, double &linear_vel, double &sign_x) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + // std::stringstream ss; + // ss << linear_vel << " "; + // limit the linear velocity by curvature + if (use_regulated_linear_velocity_scaling_) + { + const double &min_rad = regulated_linear_scaling_min_radius_; + const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad; + if (radius < min_rad) + { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + nav_2d_msgs::Twist2D cmd, result; + cmd.x = curvature_vel; + this->moveWithAccLimits(velocity, cmd, result); + curvature_vel = result.x; + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + } + } + // ss << linear_vel << " "; + // limit the linear velocity by proximity to obstacles + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(costmap_2d::NO_INFORMATION) && + pose_cost != static_cast(costmap_2d::FREE_SPACE)) + { + const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) + + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) + { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + nav_2d_msgs::Twist2D cmd, result; + cmd.x = cost_vel; + this->moveWithAccLimits(velocity, cmd, result); + cost_vel = result.x; + linear_vel = std::min(cost_vel, curvature_vel); + } + // ss << linear_vel << " "; + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop. + // This expression is eq. to + // (1) holding time to goal, t, constant using the theoretical + // lookahead distance and proposed velocity and + // (2) using t with the actual lookahead + // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). + + double dist_error_limit = costmap_ros_ != nullptr && costmap_ros_->getCostmap() != nullptr + ? 2.0 * costmap_ros_->getCostmap()->getResolution() + : 0.1; + if (dist_error > dist_error_limit) + { + double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0; + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) + { + approach_vel = min_approach_linear_velocity_; + } + else + { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + nav_2d_msgs::Twist2D cmd, result; + cmd.x = approach_vel; + this->moveWithAccLimits(velocity, cmd, result); + approach_vel = result.x; + linear_vel = std::min(linear_vel, approach_vel); + } + + // ss << linear_vel << " "; + + // Limit linear velocities to be valid + double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); + double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y); + double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y); + double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel); + linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel); + linear_vel = sign_x * linear_vel; + // ss << linear_vel << " "; + // ROS_INFO_STREAM_THROTTLE(0.1, ss.str()); +} + +std::vector +mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(geometry_msgs::Pose2D pose, std::vector footprint, double resolution) +{ + + // Dịch sang tọa độ tuyệt đối + std::vector abs_footprint; + double cos_th = std::cos(pose.theta); + double sin_th = std::sin(pose.theta); + for (auto pt : footprint) + { + pt.x *= 1.2; + pt.y *= 1.2; + geometry_msgs::Point abs_pt; + abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th; + abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th; + abs_footprint.push_back(abs_pt); + } + + std::vector points; + for (size_t i = 0; i < abs_footprint.size(); ++i) + { + const geometry_msgs::Point &start = abs_footprint[i]; + const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; + + double dx = end.x - start.x; + double dy = end.y - start.y; + double distance = std::hypot(dx, dy); + int steps = std::max(1, static_cast(std::floor(distance / resolution))); + + for (int j = 0; j <= steps; ++j) + { + if (j == steps && i != abs_footprint.size() - 1) + continue; // tránh lặp + double t = static_cast(j) / steps; + geometry_msgs::Point pt; + pt.x = start.x + t * dx; + pt.y = start.y + t * dy; + points.push_back(pt); + } + } + return points; +} + +double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, const double &y) +{ + unsigned int mx, my; + unsigned char cost; + if (costmap_ros_->getCostmap()->worldToMap(x, y, mx, my)) + { + cost = costmap_ros_->getCostmap()->getCost(mx, my); + } + return static_cast(cost); +} + +void mkt_algorithm::diff::PredictiveTrajectory::updateCostAtOffset( + TFListenerPtr tf, const std::string &robot_base_frame, const nav_2d_msgs::Pose2DStamped &base_pose, + double x_offset, double y_offset, double &cost_left, double &cost_right) +{ + nav_2d_msgs::Pose2DStamped stamped_pose, transformed_pose; + stamped_pose = base_pose; + stamped_pose.pose.x += x_offset; + stamped_pose.pose.y += y_offset; + + if (nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, transformed_pose)) + { + double cost = this->costAtPose(stamped_pose.pose.x, stamped_pose.pose.y); + if (transformed_pose.pose.y < 0) + cost_right = std::max(cost_right, cost); + else if (transformed_pose.pose.y > 0) + cost_left = std::max(cost_left, cost); + } +} + +double mkt_algorithm::diff::PredictiveTrajectory::computeDecelerationFactor(double remaining_distance, double decel_distance) +{ + if (remaining_distance >= decel_distance) + { + return 1.0; // Full speed + } + + // Smooth transition using cosine function + double ratio = fabs(remaining_distance / decel_distance); + return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0 +} + +double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const nav_2d_msgs::Pose2DStamped &carrot_pose, + double journey_plan) +{ + double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x + + carrot_pose.pose.y * carrot_pose.pose.y); + + // Avoid division by zero and handle backward motion + if (carrot_distance < 1e-3) + return journey_plan; + + // Project remaining path onto carrot direction + double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle) + return journey_plan * std::max(0.0, alignment); // Only positive projection +} + +bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::vector &angle_history, double current_angle, + double amplitude_threshold, size_t window_size) +{ + + angle_history.push_back(current_angle); + if ((unsigned int)angle_history.size() > window_size) + angle_history.erase(angle_history.begin()); + + if ((unsigned int)angle_history.size() < 2) + return false; + + double max_angle = *std::max_element(angle_history.begin(), angle_history.end()); + double min_angle = *std::min_element(angle_history.begin(), angle_history.end()); + + double amplitude = max_angle - min_angle; + // if(fabs(amplitude) > amplitude_threshold) + // ROS_INFO_THROTTLE(0.1,"%f %f %f %d %d", amplitude, max_angle , min_angle, (unsigned int)angle_history.size(), (unsigned int)window_size); + return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0; +} + +void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose2DStamped pose) +{ + const auto &plan_back_pose = compute_plan_.poses.back(); + // const double offset_max = this->min_path_distance_ + costmap_ros_->getCostmap()->getResolution() * 2.0; + // const double offset_min = this->min_path_distance_; + + auto points_rb = interpolateFootprint(pose.pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0); + for (const auto &point : points_rb) + { + double cost_goal = costAtPose(point.x, point.y); + double dx = point.x - pose.pose.x; + double dy = point.y - pose.pose.y; + double cos_yaw = cos(-pose.pose.theta); + double sin_yaw = sin(-pose.pose.theta); + double y_rel = dx * sin_yaw + dy * cos_yaw; + const double epsilon = min_path_distance_; + if (y_rel > epsilon) + { + cost_left_side_ = std::max(cost_left_side_, cost_goal); + L_.points.push_back(point); + } + else if (y_rel < -epsilon) + { + cost_right_side_ = std::max(cost_right_side_, cost_goal); + R_.points.push_back(point); + } + } + + unsigned int step_footprint = 10; + if ((unsigned int)(compute_plan_.poses.size() - 1) < 10) + { + auto points = interpolateFootprint(plan_back_pose.pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0); + for (const auto &point : points) + { + double cost_goal = costAtPose(point.x, point.y); + double dx = point.x - plan_back_pose.pose.x; + double dy = point.y - plan_back_pose.pose.y; + double cos_yaw = cos(-plan_back_pose.pose.theta); + double sin_yaw = sin(-plan_back_pose.pose.theta); + double y_rel = dx * sin_yaw + dy * cos_yaw; + const double epsilon = min_path_distance_; + if (y_rel > epsilon) + { + cost_left_goal_ = std::max(cost_left_goal_, cost_goal); + L_.points.push_back(point); + } + else if (y_rel < -epsilon) + { + cost_right_goal_ = std::max(cost_right_goal_, cost_goal); + R_.points.push_back(point); + } + else + center_cost_ = std::max(center_cost_, cost_goal); + } + } + else + { + for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint) + { + auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0); + for (const auto &point : points) + { + double cost_goal = costAtPose(point.x, point.y); + double dx = point.x - compute_plan_.poses[i].pose.x; + double dy = point.y - compute_plan_.poses[i].pose.y; + double cos_yaw = cos(-compute_plan_.poses[i].pose.theta); + double sin_yaw = sin(-compute_plan_.poses[i].pose.theta); + double y_rel = dx * sin_yaw + dy * cos_yaw; + const double epsilon = min_path_distance_; + if (y_rel > epsilon) + { + cost_left_goal_ = std::max(cost_left_goal_, cost_goal); + L_.points.push_back(point); + } + else if (y_rel < -epsilon) + { + cost_right_goal_ = std::max(cost_right_goal_, cost_goal); + R_.points.push_back(point); + } + else + center_cost_ = std::max(center_cost_, cost_goal); + } + double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x; + double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y; + if (hypot(dx, dy) > 1.0) + break; + } + } + if (enable_publish_) + { + cost_left_goals_pub_.publish(L_); + cost_right_goals_pub_.publish(R_); + } + R_.points.clear(); + L_.points.clear(); +} + +PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::PredictiveTrajectory, score_algorithm::ScoreAlgorithm) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp new file mode 100644 index 0000000..3112104 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp @@ -0,0 +1,131 @@ +#include + +// other +#include +#include +#include + +void mkt_algorithm::diff::RotateToGoal::initialize( + ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) +{ + if (!initialized_) + { + nh_ = ros::NodeHandle(nh, name); + name_ = name; + tf_ = tf; + traj_ = traj; + costmap_ros_ = costmap_ros; + this->getParams(); + + x_direction_ = y_direction_ = theta_direction_ = 0; + this->initialized_ = true; + ROS_INFO("RotateToGoal Initialized successfully"); + } +} + +void mkt_algorithm::diff::RotateToGoal::getParams() +{ + robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); + nh_priv_.param("avoid_obstacles", avoid_obstacles_, false); + nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); + nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); + nh_priv_.param("index_samples", index_samples_, 0); + nh_priv_.param("follow_step_path", follow_step_path_, false); + + nh_priv_.param("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false); + nh_priv_.param("lookahead_dist", lookahead_dist_, 0.25); + nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.3); + nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 0.9); + nh_priv_.param("lookahead_time", lookahead_time_, 1.5); + nh_priv_.param("min_journey_squared", min_journey_squared_, std::numeric_limits::infinity()); + nh_priv_.param("max_journey_squared", max_journey_squared_, std::numeric_limits::infinity()); + + nh_priv_.param("use_rotate_to_heading", use_rotate_to_heading_, false); + nh_priv_.param("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785); + + nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.0); + nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.0); + nh_priv_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0); + + // Regulated linear velocity scaling + nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); + nh_priv_.param("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9); + nh_priv_.param("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25); + + // Inflation cost scaling (Limit velocity by proximity to obstacles) + nh_priv_.param("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true); + nh_priv_.param("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0); + nh_priv_.param("cost_scaling_dist", cost_scaling_dist_, 0.6); + nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); + if (inflation_cost_scaling_factor_ <= 0.0) + { + ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + use_cost_regulated_linear_velocity_scaling_ = false; + } + double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); + control_duration_ = 1.0 / control_frequency; + if (traj_) + { + traj_.get()->getNodeHandle().param("max_vel_x", max_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_x", min_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_x", acc_lim_x_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_x", decel_lim_x_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_y", max_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_y", min_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_y", acc_lim_y_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_y", decel_lim_y_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_theta", max_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_theta", min_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_theta", acc_lim_theta_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_theta", decel_lim_theta_, 0.0); + } +} + +void mkt_algorithm::diff::RotateToGoal::reset() +{ + this->clear(); +} + +bool mkt_algorithm::diff::RotateToGoal::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, + const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, + double &x_direction, double &y_direction, double &theta_direction) +{ + + if (!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + this->getParams(); + global_plan_ = global_plan; + + goal_ = goal; + double angle = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta); + x_direction = y_direction = theta_direction = sign(angle); + ; + return true; +} + +mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator( + const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +{ + mkt_msgs::Trajectory2D result; + nav_2d_msgs::Twist2D drive_cmd; + result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; + if (!traj_) + return result; + + const double dt = control_duration_; + double vel_yaw = velocity.theta; + double ang_diff = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta); + rotateToHeading(ang_diff, velocity, drive_cmd); + drive_cmd.x = 0.0; + drive_cmd.y = 0.0; + result.velocity = drive_cmd; + return result; +} + +PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::RotateToGoal, score_algorithm::ScoreAlgorithm) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt b/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt new file mode 100644 index 0000000..0ed0625 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.10) + +# Tên dự án +project(mkt_msgs VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Cho phép các project khác include được header của mkt_msgs +set(mkt_msgs_INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/include + PARENT_SCOPE +) + +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +# Tạo INTERFACE library (header-only) +add_library(mkt_msgs INTERFACE) +target_link_libraries(mkt_msgs INTERFACE nav_2d_msgs geometry_msgs) + +# Set include directories +target_include_directories(mkt_msgs + INTERFACE + $ + $ +) + +# Cài đặt header files +install(DIRECTORY include/mkt_msgs + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +install(TARGETS mkt_msgs + EXPORT mkt_msgs-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +# Export targets +install(EXPORT mkt_msgs-targets + FILE mkt_msgs-targets.cmake + DESTINATION lib/cmake/mkt_msgs) + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME} (Header-Only)") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Headers found:") +foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") +endforeach() +message(STATUS "=================================") \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h new file mode 100644 index 0000000..5982e68 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h @@ -0,0 +1,66 @@ +// Generated by gencpp from file mkt_msgs/Trajectory2D.msg +// DO NOT EDIT! + +#ifndef MKT_MSGS_MESSAGE_TRAJECTORY2D_H +#define MKT_MSGS_MESSAGE_TRAJECTORY2D_H + +#include +#include +#include + +#include +#include + +namespace mkt_msgs +{ + template + struct Trajectory2D_ + { + typedef Trajectory2D_ Type; + + Trajectory2D_() + : velocity(), poses(), time_offsets() + { + } + Trajectory2D_(const ContainerAllocator &_alloc) + : velocity(_alloc), poses(_alloc), time_offsets(_alloc) + { + (void)_alloc; + } + + typedef ::nav_2d_msgs::Twist2D_ _velocity_type; + _velocity_type velocity; + + typedef std::vector::template rebind_alloc> _poses_type; + _poses_type poses; + + typedef std::vector::template rebind_alloc> _time_offsets_type; + _time_offsets_type time_offsets; + + typedef boost::shared_ptr<::mkt_msgs::Trajectory2D_> Ptr; + typedef boost::shared_ptr<::mkt_msgs::Trajectory2D_ const> ConstPtr; + + }; // struct Trajectory2D_ + + typedef ::mkt_msgs::Trajectory2D_> Trajectory2D; + + typedef boost::shared_ptr<::mkt_msgs::Trajectory2D> Trajectory2DPtr; + typedef boost::shared_ptr<::mkt_msgs::Trajectory2D const> Trajectory2DConstPtr; + + template + bool operator==(const ::mkt_msgs::Trajectory2D_ &lhs, const ::mkt_msgs::Trajectory2D_ &rhs) + { + return lhs.velocity == rhs.velocity && + lhs.poses == rhs.poses && + lhs.time_offsets == rhs.time_offsets; + } + + template + bool operator!=(const ::mkt_msgs::Trajectory2D_ &lhs, const ::mkt_msgs::Trajectory2D_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace mkt_msgs + +#endif // MKT_MSGS_MESSAGE_TRAJECTORY2D_H diff --git a/src/Libraries/nav_2d_msgs/CMakeLists.txt b/src/Libraries/nav_2d_msgs/CMakeLists.txt new file mode 100755 index 0000000..ae2aefd --- /dev/null +++ b/src/Libraries/nav_2d_msgs/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.10) + +project(nav_2d_msgs VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# Tìm tất cả header files +file(GLOB_RECURSE HEADERS "include/nav_2d_msgs/*.h") + +# Tạo INTERFACE library (header-only) +add_library(nav_2d_msgs INTERFACE) + +# Set include directories +target_include_directories(nav_2d_msgs + INTERFACE + $ + $ +) + +# Link dependencies (header-only, chỉ cần include paths) +# Các dependencies này cũng là header-only từ common_msgs +target_link_libraries(nav_2d_msgs + INTERFACE + std_msgs + geometry_msgs +) + +# Cài đặt header files +install(DIRECTORY include/nav_2d_msgs + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +# Cài đặt target +install(TARGETS nav_2d_msgs + EXPORT nav_2d_msgs-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# Export targets +# Bây giờ có thể export dependencies vì std_msgs và geometry_msgs đã được export +install(EXPORT nav_2d_msgs-targets + FILE nav_2d_msgs-targets.cmake + NAMESPACE nav_2d_msgs:: + DESTINATION lib/cmake/nav_2d_msgs) + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME} (Header-Only)") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Headers found:") +foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") +endforeach() +message(STATUS "Dependencies: std_msgs, geometry_msgs") +message(STATUS "=================================") + diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/ComplexPolygon2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/ComplexPolygon2D.h new file mode 100644 index 0000000..b4ca539 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/ComplexPolygon2D.h @@ -0,0 +1,74 @@ +// Generated by gencpp from file nav_2d_msgs/ComplexPolygon2D.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H +#define NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H + + +#include +#include +#include + +#include +#include + +namespace nav_2d_msgs +{ +template +struct ComplexPolygon2D_ +{ + typedef ComplexPolygon2D_ Type; + + ComplexPolygon2D_() + : outer() + , inner() { + } + ComplexPolygon2D_(const ContainerAllocator& _alloc) + : outer(_alloc) + , inner(_alloc) { + (void)_alloc; + } + + + + typedef ::nav_2d_msgs::Polygon2D_ _outer_type; + _outer_type outer; + + typedef std::vector< ::nav_2d_msgs::Polygon2D_ , typename std::allocator_traits::template rebind_alloc< ::nav_2d_msgs::Polygon2D_ >> _inner_type; + _inner_type inner; + + + + + + typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_ const> ConstPtr; + +}; // struct ComplexPolygon2D_ + +typedef ::nav_2d_msgs::ComplexPolygon2D_ > ComplexPolygon2D; + +typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D > ComplexPolygon2DPtr; +typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D const> ComplexPolygon2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::ComplexPolygon2D_ & lhs, const ::nav_2d_msgs::ComplexPolygon2D_ & rhs) +{ + return lhs.outer == rhs.outer && + lhs.inner == rhs.inner; +} + +template +bool operator!=(const ::nav_2d_msgs::ComplexPolygon2D_ & lhs, const ::nav_2d_msgs::ComplexPolygon2D_ & rhs) +{ + return !(lhs == rhs); +} + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridInfo.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridInfo.h new file mode 100644 index 0000000..91adb59 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridInfo.h @@ -0,0 +1,94 @@ +// Generated by gencpp from file nav_2d_msgs/NavGridInfo.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H +#define NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H + +#include +#include +#include + +namespace nav_2d_msgs +{ +template +struct NavGridInfo_ +{ + typedef NavGridInfo_ Type; + + NavGridInfo_() + : width(0) + , height(0) + , resolution(0.0) + , frame_id() + , origin_x(0.0) + , origin_y(0.0) { + } + NavGridInfo_(const ContainerAllocator& _alloc) + : width(0) + , height(0) + , resolution(0.0) + , frame_id(_alloc) + , origin_x(0.0) + , origin_y(0.0) { + (void)_alloc; + } + + + + typedef uint32_t _width_type; + _width_type width; + + typedef uint32_t _height_type; + _height_type height; + + typedef double _resolution_type; + _resolution_type resolution; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _frame_id_type; + _frame_id_type frame_id; + + typedef double _origin_x_type; + _origin_x_type origin_x; + + typedef double _origin_y_type; + _origin_y_type origin_y; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_ const> ConstPtr; + +}; // struct NavGridInfo_ + +typedef ::nav_2d_msgs::NavGridInfo_ > NavGridInfo; + +typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo > NavGridInfoPtr; +typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo const> NavGridInfoConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::NavGridInfo_ & lhs, const ::nav_2d_msgs::NavGridInfo_ & rhs) +{ + return lhs.width == rhs.width && + lhs.height == rhs.height && + lhs.resolution == rhs.resolution && + lhs.frame_id == rhs.frame_id && + lhs.origin_x == rhs.origin_x && + lhs.origin_y == rhs.origin_y; +} + +template +bool operator!=(const ::nav_2d_msgs::NavGridInfo_ & lhs, const ::nav_2d_msgs::NavGridInfo_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfChars.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfChars.h new file mode 100644 index 0000000..9603f07 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfChars.h @@ -0,0 +1,80 @@ +// Generated by gencpp from file nav_2d_msgs/NavGridOfChars.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H +#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H + + +#include +#include +#include + +#include +#include + +namespace nav_2d_msgs +{ +template +struct NavGridOfChars_ +{ + typedef NavGridOfChars_ Type; + + NavGridOfChars_() + : stamp() + , info() + , data() { + } + NavGridOfChars_(const ContainerAllocator& _alloc) + : stamp() + , info(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef robot::Time _stamp_type; + _stamp_type stamp; + + typedef ::nav_2d_msgs::NavGridInfo_ _info_type; + _info_type info; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_ const> ConstPtr; + +}; // struct NavGridOfChars_ + +typedef ::nav_2d_msgs::NavGridOfChars_ > NavGridOfChars; + +typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars > NavGridOfCharsPtr; +typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars const> NavGridOfCharsConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::NavGridOfChars_ & lhs, const ::nav_2d_msgs::NavGridOfChars_ & rhs) +{ + return lhs.stamp == rhs.stamp && + lhs.info == rhs.info && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::nav_2d_msgs::NavGridOfChars_ & lhs, const ::nav_2d_msgs::NavGridOfChars_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfCharsUpdate.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfCharsUpdate.h new file mode 100644 index 0000000..e892439 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfCharsUpdate.h @@ -0,0 +1,80 @@ +// Generated by gencpp from file nav_2d_msgs/NavGridOfCharsUpdate.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H +#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H + + +#include +#include +#include + +#include +#include + +namespace nav_2d_msgs +{ +template +struct NavGridOfCharsUpdate_ +{ + typedef NavGridOfCharsUpdate_ Type; + + NavGridOfCharsUpdate_() + : stamp() + , bounds() + , data() { + } + NavGridOfCharsUpdate_(const ContainerAllocator& _alloc) + : stamp() + , bounds(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef robot::Time _stamp_type; + _stamp_type stamp; + + typedef ::nav_2d_msgs::UIntBounds_ _bounds_type; + _bounds_type bounds; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_ const> ConstPtr; + +}; // struct NavGridOfCharsUpdate_ + +typedef ::nav_2d_msgs::NavGridOfCharsUpdate_ > NavGridOfCharsUpdate; + +typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate > NavGridOfCharsUpdatePtr; +typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate const> NavGridOfCharsUpdateConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::NavGridOfCharsUpdate_ & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_ & rhs) +{ + return lhs.stamp == rhs.stamp && + lhs.bounds == rhs.bounds && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::nav_2d_msgs::NavGridOfCharsUpdate_ & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoubles.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoubles.h new file mode 100644 index 0000000..f2829b0 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoubles.h @@ -0,0 +1,80 @@ +// Generated by gencpp from file nav_2d_msgs/NavGridOfDoubles.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H +#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H + + +#include +#include +#include + +#include +#include + +namespace nav_2d_msgs +{ +template +struct NavGridOfDoubles_ +{ + typedef NavGridOfDoubles_ Type; + + NavGridOfDoubles_() + : stamp() + , info() + , data() { + } + NavGridOfDoubles_(const ContainerAllocator& _alloc) + : stamp() + , info(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef robot::Time _stamp_type; + _stamp_type stamp; + + typedef ::nav_2d_msgs::NavGridInfo_ _info_type; + _info_type info; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_ const> ConstPtr; + +}; // struct NavGridOfDoubles_ + +typedef ::nav_2d_msgs::NavGridOfDoubles_ > NavGridOfDoubles; + +typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles > NavGridOfDoublesPtr; +typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles const> NavGridOfDoublesConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::NavGridOfDoubles_ & lhs, const ::nav_2d_msgs::NavGridOfDoubles_ & rhs) +{ + return lhs.stamp == rhs.stamp && + lhs.info == rhs.info && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::nav_2d_msgs::NavGridOfDoubles_ & lhs, const ::nav_2d_msgs::NavGridOfDoubles_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoublesUpdate.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoublesUpdate.h new file mode 100644 index 0000000..1706c71 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoublesUpdate.h @@ -0,0 +1,80 @@ +// Generated by gencpp from file nav_2d_msgs/NavGridOfDoublesUpdate.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H +#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H + + +#include +#include +#include + +#include +#include + +namespace nav_2d_msgs +{ +template +struct NavGridOfDoublesUpdate_ +{ + typedef NavGridOfDoublesUpdate_ Type; + + NavGridOfDoublesUpdate_() + : stamp() + , bounds() + , data() { + } + NavGridOfDoublesUpdate_(const ContainerAllocator& _alloc) + : stamp() + , bounds(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef robot::Time _stamp_type; + _stamp_type stamp; + + typedef ::nav_2d_msgs::UIntBounds_ _bounds_type; + _bounds_type bounds; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_ const> ConstPtr; + +}; // struct NavGridOfDoublesUpdate_ + +typedef ::nav_2d_msgs::NavGridOfDoublesUpdate_ > NavGridOfDoublesUpdate; + +typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate > NavGridOfDoublesUpdatePtr; +typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate const> NavGridOfDoublesUpdateConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::NavGridOfDoublesUpdate_ & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_ & rhs) +{ + return lhs.stamp == rhs.stamp && + lhs.bounds == rhs.bounds && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::nav_2d_msgs::NavGridOfDoublesUpdate_ & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Path2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Path2D.h new file mode 100644 index 0000000..4f0b967 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Path2D.h @@ -0,0 +1,74 @@ +// Generated by gencpp from file nav_2d_msgs/Path2D.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_PATH2D_H +#define NAV_2D_MSGS_MESSAGE_PATH2D_H + + +#include +#include +#include + +#include +#include + +namespace nav_2d_msgs +{ +template +struct Path2D_ +{ + typedef Path2D_ Type; + + Path2D_() + : header() + , poses() { + } + Path2D_(const ContainerAllocator& _alloc) + : header() + , poses(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef std::vector< ::nav_2d_msgs::Pose2DStamped_ , typename std::allocator_traits::template rebind_alloc< ::nav_2d_msgs::Pose2DStamped_ >> _poses_type; + _poses_type poses; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Path2D_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Path2D_ const> ConstPtr; + +}; // struct Path2D_ + +typedef ::nav_2d_msgs::Path2D_ > Path2D; + +typedef std::shared_ptr< ::nav_2d_msgs::Path2D > Path2DPtr; +typedef std::shared_ptr< ::nav_2d_msgs::Path2D const> Path2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Path2D_ & lhs, const ::nav_2d_msgs::Path2D_ & rhs) +{ + return lhs.header == rhs.header && + lhs.poses == rhs.poses; +} + +template +bool operator!=(const ::nav_2d_msgs::Path2D_ & lhs, const ::nav_2d_msgs::Path2D_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_PATH2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Point2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Point2D.h new file mode 100644 index 0000000..f192042 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Point2D.h @@ -0,0 +1,72 @@ +// Generated by gencpp from file nav_2d_msgs/Point2D.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_POINT2D_H +#define NAV_2D_MSGS_MESSAGE_POINT2D_H + + +#include +#include +#include + + +namespace nav_2d_msgs +{ +template +struct Point2D_ +{ + typedef Point2D_ Type; + + Point2D_() + : x(0.0) + , y(0.0) { + } + Point2D_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Point2D_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Point2D_ const> ConstPtr; + +}; // struct Point2D_ + +typedef ::nav_2d_msgs::Point2D_ > Point2D; + +typedef std::shared_ptr< ::nav_2d_msgs::Point2D > Point2DPtr; +typedef std::shared_ptr< ::nav_2d_msgs::Point2D const> Point2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Point2D_ & lhs, const ::nav_2d_msgs::Point2D_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y; +} + +template +bool operator!=(const ::nav_2d_msgs::Point2D_ & lhs, const ::nav_2d_msgs::Point2D_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_POINT2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2D.h new file mode 100644 index 0000000..934e145 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2D.h @@ -0,0 +1,67 @@ +// Generated by gencpp from file nav_2d_msgs/Polygon2D.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2D_H +#define NAV_2D_MSGS_MESSAGE_POLYGON2D_H + + +#include +#include +#include + +#include + +namespace nav_2d_msgs +{ +template +struct Polygon2D_ +{ + typedef Polygon2D_ Type; + + Polygon2D_() + : points() { + } + Polygon2D_(const ContainerAllocator& _alloc) + : points(_alloc) { + (void)_alloc; + } + + + + typedef std::vector< ::nav_2d_msgs::Point2D_ , typename std::allocator_traits::template rebind_alloc< ::nav_2d_msgs::Point2D_ >> _points_type; + _points_type points; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_ const> ConstPtr; + +}; // struct Polygon2D_ + +typedef ::nav_2d_msgs::Polygon2D_ > Polygon2D; + +typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D > Polygon2DPtr; +typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D const> Polygon2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Polygon2D_ & lhs, const ::nav_2d_msgs::Polygon2D_ & rhs) +{ + return lhs.points == rhs.points; +} + +template +bool operator!=(const ::nav_2d_msgs::Polygon2D_ & lhs, const ::nav_2d_msgs::Polygon2D_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_POLYGON2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DCollection.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DCollection.h new file mode 100644 index 0000000..c528e9f --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DCollection.h @@ -0,0 +1,81 @@ +// Generated by gencpp from file nav_2d_msgs/Polygon2DCollection.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H +#define NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H + + +#include +#include +#include + +#include +#include +#include + +namespace nav_2d_msgs +{ +template +struct Polygon2DCollection_ +{ + typedef Polygon2DCollection_ Type; + + Polygon2DCollection_() + : header() + , polygons() + , colors() { + } + Polygon2DCollection_(const ContainerAllocator& _alloc) + : header() + , polygons(_alloc) + , colors(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef std::vector< ::nav_2d_msgs::ComplexPolygon2D_ , typename std::allocator_traits::template rebind_alloc< ::nav_2d_msgs::ComplexPolygon2D_ >> _polygons_type; + _polygons_type polygons; + + typedef std::vector< ::std_msgs::ColorRGBA , typename std::allocator_traits::template rebind_alloc< ::std_msgs::ColorRGBA >> _colors_type; + _colors_type colors; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_ const> ConstPtr; + +}; // struct Polygon2DCollection_ + +typedef ::nav_2d_msgs::Polygon2DCollection_ > Polygon2DCollection; + +typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection > Polygon2DCollectionPtr; +typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection const> Polygon2DCollectionConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Polygon2DCollection_ & lhs, const ::nav_2d_msgs::Polygon2DCollection_ & rhs) +{ + return lhs.header == rhs.header && + lhs.polygons == rhs.polygons && + lhs.colors == rhs.colors; +} + +template +bool operator!=(const ::nav_2d_msgs::Polygon2DCollection_ & lhs, const ::nav_2d_msgs::Polygon2DCollection_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DStamped.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DStamped.h new file mode 100644 index 0000000..bb34d54 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DStamped.h @@ -0,0 +1,74 @@ +// Generated by gencpp from file nav_2d_msgs/Polygon2DStamped.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H +#define NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H + + +#include +#include +#include + +#include +#include + +namespace nav_2d_msgs +{ +template +struct Polygon2DStamped_ +{ + typedef Polygon2DStamped_ Type; + + Polygon2DStamped_() + : header() + , polygon() { + } + Polygon2DStamped_(const ContainerAllocator& _alloc) + : header() + , polygon(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef ::nav_2d_msgs::Polygon2D_ _polygon_type; + _polygon_type polygon; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_ const> ConstPtr; + +}; // struct Polygon2DStamped_ + +typedef ::nav_2d_msgs::Polygon2DStamped_ > Polygon2DStamped; + +typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped > Polygon2DStampedPtr; +typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped const> Polygon2DStampedConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Polygon2DStamped_ & lhs, const ::nav_2d_msgs::Polygon2DStamped_ & rhs) +{ + return lhs.header == rhs.header && + lhs.polygon == rhs.polygon; +} + +template +bool operator!=(const ::nav_2d_msgs::Polygon2DStamped_ & lhs, const ::nav_2d_msgs::Polygon2DStamped_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2D32.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2D32.h new file mode 100644 index 0000000..367c668 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2D32.h @@ -0,0 +1,78 @@ +// Generated by gencpp from file nav_2d_msgs/Pose2D32.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_POSE2D32_H +#define NAV_2D_MSGS_MESSAGE_POSE2D32_H + + +#include +#include +#include + + +namespace nav_2d_msgs +{ +template +struct Pose2D32_ +{ + typedef Pose2D32_ Type; + + Pose2D32_() + : x(0.0) + , y(0.0) + , theta(0.0) { + } + Pose2D32_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , theta(0.0) { + (void)_alloc; + } + + + + typedef float _x_type; + _x_type x; + + typedef float _y_type; + _y_type y; + + typedef float _theta_type; + _theta_type theta; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32_ const> ConstPtr; + +}; // struct Pose2D32_ + +typedef ::nav_2d_msgs::Pose2D32_ > Pose2D32; + +typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32 > Pose2D32Ptr; +typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32 const> Pose2D32ConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Pose2D32_ & lhs, const ::nav_2d_msgs::Pose2D32_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.theta == rhs.theta; +} + +template +bool operator!=(const ::nav_2d_msgs::Pose2D32_ & lhs, const ::nav_2d_msgs::Pose2D32_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_POSE2D32_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2DStamped.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2DStamped.h new file mode 100644 index 0000000..f8ebac1 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2DStamped.h @@ -0,0 +1,74 @@ +// Generated by gencpp from file nav_2d_msgs/Pose2DStamped.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H +#define NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H + + +#include +#include +#include + +#include +#include + +namespace nav_2d_msgs +{ +template +struct Pose2DStamped_ +{ + typedef Pose2DStamped_ Type; + + Pose2DStamped_() + : header() + , pose() { + } + Pose2DStamped_(const ContainerAllocator& _alloc) + : header() + , pose() { + (void)_alloc; + } + + + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef ::geometry_msgs::Pose2D _pose_type; + _pose_type pose; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped_ const> ConstPtr; + +}; // struct Pose2DStamped_ + +typedef ::nav_2d_msgs::Pose2DStamped_ > Pose2DStamped; + +typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped > Pose2DStampedPtr; +typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped const> Pose2DStampedConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Pose2DStamped_ & lhs, const ::nav_2d_msgs::Pose2DStamped_ & rhs) +{ + return lhs.header == rhs.header && + lhs.pose == rhs.pose; +} + +template +bool operator!=(const ::nav_2d_msgs::Pose2DStamped_ & lhs, const ::nav_2d_msgs::Pose2DStamped_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPlugin.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPlugin.h new file mode 100644 index 0000000..9d215b3 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPlugin.h @@ -0,0 +1,30 @@ +// Generated by gencpp from file nav_2d_msgs/SwitchPlugin.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H +#define NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H + + +#include +#include + + +namespace nav_2d_msgs +{ + +struct SwitchPlugin +{ + +typedef SwitchPluginRequest Request; +typedef SwitchPluginResponse Response; +Request request; +Response response; + +typedef Request RequestType; +typedef Response ResponseType; + +}; // struct SwitchPlugin +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginRequest.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginRequest.h new file mode 100644 index 0000000..82eae9d --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginRequest.h @@ -0,0 +1,66 @@ +// Generated by gencpp from file nav_2d_msgs/SwitchPluginRequest.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H +#define NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H + + +#include +#include +#include + + +namespace nav_2d_msgs +{ +template +struct SwitchPluginRequest_ +{ + typedef SwitchPluginRequest_ Type; + + SwitchPluginRequest_() + : new_plugin() { + } + SwitchPluginRequest_(const ContainerAllocator& _alloc) + : new_plugin(_alloc) { + (void)_alloc; + } + + + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _new_plugin_type; + _new_plugin_type new_plugin; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest_ const> ConstPtr; + +}; // struct SwitchPluginRequest_ + +typedef ::nav_2d_msgs::SwitchPluginRequest_ > SwitchPluginRequest; + +typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest > SwitchPluginRequestPtr; +typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest const> SwitchPluginRequestConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::SwitchPluginRequest_ & lhs, const ::nav_2d_msgs::SwitchPluginRequest_ & rhs) +{ + return lhs.new_plugin == rhs.new_plugin; +} + +template +bool operator!=(const ::nav_2d_msgs::SwitchPluginRequest_ & lhs, const ::nav_2d_msgs::SwitchPluginRequest_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginResponse.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginResponse.h new file mode 100644 index 0000000..295f8de --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginResponse.h @@ -0,0 +1,72 @@ +// Generated by gencpp from file nav_2d_msgs/SwitchPluginResponse.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H +#define NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H + + +#include +#include +#include + + +namespace nav_2d_msgs +{ +template +struct SwitchPluginResponse_ +{ + typedef SwitchPluginResponse_ Type; + + SwitchPluginResponse_() + : success(false) + , message() { + } + SwitchPluginResponse_(const ContainerAllocator& _alloc) + : success(false) + , message(_alloc) { + (void)_alloc; + } + + + + typedef uint8_t _success_type; + _success_type success; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _message_type; + _message_type message; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse_ const> ConstPtr; + +}; // struct SwitchPluginResponse_ + +typedef ::nav_2d_msgs::SwitchPluginResponse_ > SwitchPluginResponse; + +typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse > SwitchPluginResponsePtr; +typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse const> SwitchPluginResponseConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::SwitchPluginResponse_ & lhs, const ::nav_2d_msgs::SwitchPluginResponse_ & rhs) +{ + return lhs.success == rhs.success && + lhs.message == rhs.message; +} + +template +bool operator!=(const ::nav_2d_msgs::SwitchPluginResponse_ & lhs, const ::nav_2d_msgs::SwitchPluginResponse_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D.h new file mode 100644 index 0000000..120c6cc --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D.h @@ -0,0 +1,78 @@ +// Generated by gencpp from file nav_2d_msgs/Twist2D.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_TWIST2D_H +#define NAV_2D_MSGS_MESSAGE_TWIST2D_H + + +#include +#include +#include + + +namespace nav_2d_msgs +{ +template +struct Twist2D_ +{ + typedef Twist2D_ Type; + + Twist2D_() + : x(0.0) + , y(0.0) + , theta(0.0) { + } + Twist2D_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , theta(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + typedef double _theta_type; + _theta_type theta; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Twist2D_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Twist2D_ const> ConstPtr; + +}; // struct Twist2D_ + +typedef ::nav_2d_msgs::Twist2D_ > Twist2D; + +typedef std::shared_ptr< ::nav_2d_msgs::Twist2D > Twist2DPtr; +typedef std::shared_ptr< ::nav_2d_msgs::Twist2D const> Twist2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Twist2D_ & lhs, const ::nav_2d_msgs::Twist2D_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.theta == rhs.theta; +} + +template +bool operator!=(const ::nav_2d_msgs::Twist2D_ & lhs, const ::nav_2d_msgs::Twist2D_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_TWIST2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D32.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D32.h new file mode 100644 index 0000000..1c56809 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D32.h @@ -0,0 +1,78 @@ +// Generated by gencpp from file nav_2d_msgs/Twist2D32.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_TWIST2D32_H +#define NAV_2D_MSGS_MESSAGE_TWIST2D32_H + + +#include +#include +#include + + +namespace nav_2d_msgs +{ +template +struct Twist2D32_ +{ + typedef Twist2D32_ Type; + + Twist2D32_() + : x(0.0) + , y(0.0) + , theta(0.0) { + } + Twist2D32_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , theta(0.0) { + (void)_alloc; + } + + + + typedef float _x_type; + _x_type x; + + typedef float _y_type; + _y_type y; + + typedef float _theta_type; + _theta_type theta; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32_ const> ConstPtr; + +}; // struct Twist2D32_ + +typedef ::nav_2d_msgs::Twist2D32_ > Twist2D32; + +typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32 > Twist2D32Ptr; +typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32 const> Twist2D32ConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Twist2D32_ & lhs, const ::nav_2d_msgs::Twist2D32_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.theta == rhs.theta; +} + +template +bool operator!=(const ::nav_2d_msgs::Twist2D32_ & lhs, const ::nav_2d_msgs::Twist2D32_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_TWIST2D32_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2DStamped.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2DStamped.h new file mode 100644 index 0000000..5cb5620 --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2DStamped.h @@ -0,0 +1,74 @@ +// Generated by gencpp from file nav_2d_msgs/Twist2DStamped.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H +#define NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H + + +#include +#include +#include + +#include +#include + +namespace nav_2d_msgs +{ +template +struct Twist2DStamped_ +{ + typedef Twist2DStamped_ Type; + + Twist2DStamped_() + : header() + , velocity() { + } + Twist2DStamped_(const ContainerAllocator& _alloc) + : header() + , velocity(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef ::nav_2d_msgs::Twist2D_ _velocity_type; + _velocity_type velocity; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped_ const> ConstPtr; + +}; // struct Twist2DStamped_ + +typedef ::nav_2d_msgs::Twist2DStamped_ > Twist2DStamped; + +typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped > Twist2DStampedPtr; +typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped const> Twist2DStampedConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::Twist2DStamped_ & lhs, const ::nav_2d_msgs::Twist2DStamped_ & rhs) +{ + return lhs.header == rhs.header && + lhs.velocity == rhs.velocity; +} + +template +bool operator!=(const ::nav_2d_msgs::Twist2DStamped_ & lhs, const ::nav_2d_msgs::Twist2DStamped_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/UIntBounds.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/UIntBounds.h new file mode 100644 index 0000000..0a006ae --- /dev/null +++ b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/UIntBounds.h @@ -0,0 +1,84 @@ +// Generated by gencpp from file nav_2d_msgs/UIntBounds.msg +// DO NOT EDIT! + + +#ifndef NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H +#define NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H + + +#include +#include +#include + + +namespace nav_2d_msgs +{ +template +struct UIntBounds_ +{ + typedef UIntBounds_ Type; + + UIntBounds_() + : min_x(0) + , min_y(0) + , max_x(0) + , max_y(0) { + } + UIntBounds_(const ContainerAllocator& _alloc) + : min_x(0) + , min_y(0) + , max_x(0) + , max_y(0) { + (void)_alloc; + } + + + + typedef uint32_t _min_x_type; + _min_x_type min_x; + + typedef uint32_t _min_y_type; + _min_y_type min_y; + + typedef uint32_t _max_x_type; + _max_x_type max_x; + + typedef uint32_t _max_y_type; + _max_y_type max_y; + + + + + typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_ > Ptr; + typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_ const> ConstPtr; + +}; // struct UIntBounds_ + +typedef ::nav_2d_msgs::UIntBounds_ > UIntBounds; + +typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds > UIntBoundsPtr; +typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds const> UIntBoundsConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::nav_2d_msgs::UIntBounds_ & lhs, const ::nav_2d_msgs::UIntBounds_ & rhs) +{ + return lhs.min_x == rhs.min_x && + lhs.min_y == rhs.min_y && + lhs.max_x == rhs.max_x && + lhs.max_y == rhs.max_y; +} + +template +bool operator!=(const ::nav_2d_msgs::UIntBounds_ & lhs, const ::nav_2d_msgs::UIntBounds_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_2d_msgs + +#endif // NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H diff --git a/src/Libraries/nav_2d_utils/CMakeLists.txt b/src/Libraries/nav_2d_utils/CMakeLists.txt new file mode 100755 index 0000000..d7b3e7f --- /dev/null +++ b/src/Libraries/nav_2d_utils/CMakeLists.txt @@ -0,0 +1,146 @@ +cmake_minimum_required(VERSION 3.10) + +project(nav_2d_utils VERSION 1.0.0 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# Enable Position Independent Code +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Find dependencies +find_package(console_bridge REQUIRED) +find_package(Boost REQUIRED COMPONENTS system thread) +find_package(xmlrpcpp QUIET) +if(NOT xmlrpcpp_FOUND) + # Try alternative package name + find_package(XmlRpcCpp QUIET) +endif() + +# Libraries +add_library(conversions src/conversions.cpp) +target_include_directories(conversions + PUBLIC + $ + $ +) +target_link_libraries(conversions + PUBLIC + nav_2d_msgs + geometry_msgs + nav_msgs + nav_grid + nav_core2 + PRIVATE + console_bridge::console_bridge + Boost::system + Boost::thread +) + +add_library(path_ops src/path_ops.cpp) +target_include_directories(path_ops + PUBLIC + $ + $ +) +target_link_libraries(path_ops + PUBLIC + nav_2d_msgs + geometry_msgs +) + +add_library(polygons src/polygons.cpp src/footprint.cpp) +target_include_directories(polygons + PUBLIC + $ + $ +) +target_link_libraries(polygons + PUBLIC + nav_2d_msgs + geometry_msgs + PRIVATE + ${xmlrpcpp_LIBRARIES} +) + +if(xmlrpcpp_FOUND) + target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS}) +elseif(XmlRpcCpp_FOUND) + target_include_directories(polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS}) + target_link_libraries(polygons PRIVATE ${XmlRpcCpp_LIBRARIES}) +endif() + +add_library(bounds src/bounds.cpp) +target_include_directories(bounds + PUBLIC + $ + $ +) +target_link_libraries(bounds + PUBLIC + nav_grid + nav_core2 +) + +add_library(tf_help src/tf_help.cpp) +target_include_directories(tf_help + PUBLIC + $ + $ +) +target_link_libraries(tf_help + PUBLIC + nav_2d_msgs + geometry_msgs + nav_core2 + tf3 +) + +# Create an INTERFACE library that represents all nav_2d_utils libraries +add_library(nav_2d_utils INTERFACE) +target_include_directories(nav_2d_utils + INTERFACE + $ + $ +) +target_link_libraries(nav_2d_utils + INTERFACE + conversions + path_ops + polygons + bounds + tf_help +) + +# Install header files +install(DIRECTORY include/nav_2d_utils + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +# Install mapbox headers +install(DIRECTORY include/mapbox + DESTINATION include + FILES_MATCHING PATTERN "*.hpp") + +# Install targets +install(TARGETS nav_2d_utils conversions path_ops polygons bounds tf_help + EXPORT nav_2d_utils-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# Export targets +install(EXPORT nav_2d_utils-targets + FILE nav_2d_utils-targets.cmake + NAMESPACE nav_2d_utils:: + DESTINATION lib/cmake/nav_2d_utils) + +# Print configuration info +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Libraries: conversions, path_ops, polygons, bounds, tf_help") +message(STATUS "Dependencies: nav_2d_msgs, geometry_msgs, nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost") +message(STATUS "=================================") diff --git a/src/Libraries/nav_2d_utils/README.md b/src/Libraries/nav_2d_utils/README.md new file mode 100755 index 0000000..c71f8a7 --- /dev/null +++ b/src/Libraries/nav_2d_utils/README.md @@ -0,0 +1,10 @@ +# nav_2d_utils +A handful of useful utility functions for nav_core2 packages. + * Bounds - Utilities for `nav_core2::Bounds` objects interacting with other messages/types and for dividing a `UIntBounds` into multiple smaller bounds. + * [Conversions](doc/Conversions.md) - Tools for converting between `nav_2d_msgs` and other types. + * OdomSubscriber - subscribes to the standard `nav_msgs::Odometry` message and provides access to the velocity component as a `nav_2d_msgs::Twist` + * Parameters - a couple ROS parameter patterns + * PathOps - functions for working with `nav_2d_msgs::Path2D` objects (beyond strict conversion) + * [Plugin Mux](doc/PluginMux.md) - tool for switching between multiple `pluginlib` plugins + * [Polygons and Footprints](doc/PolygonsAndFootprints.md) - functions for working with `Polygon2D` objects + * TF Help - Tools for transforming `nav_2d_msgs` and other common operations. diff --git a/src/Libraries/nav_2d_utils/doc/Conversions.md b/src/Libraries/nav_2d_utils/doc/Conversions.md new file mode 100755 index 0000000..d8b0060 --- /dev/null +++ b/src/Libraries/nav_2d_utils/doc/Conversions.md @@ -0,0 +1,54 @@ +# nav_2d_utils Conversions + +(note: exact syntax differs from below for conciseness, leaving out `const` and `&`) + +## Twist Transformations +| to `nav_2d_msgs` | from `nav_2d_msgs` | +| -- | -- | +| `Twist2D twist3Dto2D(geometry_msgs::Twist)` | `geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)` + +## Point Transformations +| to `nav_2d_msgs` | from `nav_2d_msgs` | +| -- | -- | +| `Point2D pointToPoint2D(geometry_msgs::Point)` | `geometry_msgs::Point pointToPoint3D(Point2D)` +| `Point2D pointToPoint2D(geometry_msgs::Point32)` | `geometry_msgs::Point32 pointToPoint32(Point2D)` + +## Pose Transformations +| to `nav_2d_msgs` | from `nav_2d_msgs` | +| -- | -- | +| `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)` +||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, ros::Time)`| +||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`| +| `Pose2DStamped stampedPoseToPose2D(tf::Stamped)` | | + +## Path Transformations +| to `nav_2d_msgs` | from `nav_2d_msgs` | +| -- | -- | +| `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)` +| `Path2D posesToPath2D(std::vector)` | `nav_msgs::Path poses2DToPath(std::vector, std::string, ros::Time)` + +Also, `nav_msgs::Path posesToPath(std::vector)` + +## Polygon Transformations +| to `nav_2d_msgs` | from `nav_2d_msgs` | +| -- | -- | +| `Polygon2D polygon3Dto2D(geometry_msgs::Polygon)` |`geometry_msgs::Polygon polygon2Dto3D(Polygon2D)` +| `Polygon2DStamped polygon3Dto2D(geometry_msgs::PolygonStamped)` | `geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)` + +## Info Transformations +| to `nav_2d_msgs` | from `nav_2d_msgs` | +| -- | -- | +|`nav_2d_msgs::NavGridInfo toMsg(nav_grid::NavGridInfo)`|`nav_grid::NavGridInfo fromMsg(nav_2d_msgs::NavGridInfo)`| + +| to `nav_grid` info | from `nav_grid` info | +| -- | -- | +|`nav_grid::NavGridInfo infoToInfo(nav_msgs::MapMetaData, std::string)` | `nav_msgs::MapMetaData infoToInfo(nav_grid::NavGridInfo)` + +| to two dimensional pose | to three dimensional pose | +| -- | -- | +| `Pose2D getOrigin2D(nav_grid::NavGridInfo)` | `geometry_msgs::Pose getOrigin3D(nav_grid::NavGridInfo)`| + +## Bounds Transformations +| to `nav_2d_msgs` | from `nav_2d_msgs` | +| -- | -- | +|`nav_2d_msgs::UIntBounds toMsg(nav_core2::UIntBounds)`|`nav_core2::UIntBounds fromMsg(nav_2d_msgs::UIntBounds)`| diff --git a/src/Libraries/nav_2d_utils/doc/PluginMux.md b/src/Libraries/nav_2d_utils/doc/PluginMux.md new file mode 100755 index 0000000..6872fbe --- /dev/null +++ b/src/Libraries/nav_2d_utils/doc/PluginMux.md @@ -0,0 +1,39 @@ + +# Plugin Mux +`PluginMux` is an organizer for switching between multiple different plugins of the same type. It may be easiest to see how it operates through an example. Let's say we have multiple global planners we would like to use at different times, with only one being active at a given time. + +This means we have multiple `pluginlib` plugins to load that extend the `BaseGlobalPlanner` interface from the `nav_core` package. We define multiple namespaces in the `global_planner_namespaces` and load each of them. Here's an example parameter config. + +``` +global_planner_namespaces: + - boring_nav_fn + - wacky_global_planner +boring_nav_fn: + allow_unknown: true + plugin_class: navfn/NavfnROS +wacky_global_planner: + allow_unknown: false + # default value commented out + # plugin_class: global_planner/GlobalPlanner +``` + +The namespaces are arbitrary strings, and need not reflect the name of the planner. The package and class name for the plugin will be specified by the `plugin_class` parameter. By default, the first namespace will be loaded as the current plugin. + +To advertise which plugin is active, we publish the namespace on a latched topic and set a parameter with the same name (`~/current_global_planner`). We can then switch among them with a `SetString` ROS service call, or the `usePlugin` C++ method. + +This configuration is all created by creating a plugin mux with the following parameters (parameter names shown for convenience): +``` +PluginMux(plugin_package = "nav_core", + plugin_class = "BaseGlobalPlanner", + parameter_name = "global_planner_namespaces", + default_value = "global_planner/GlobalPlanner", + ros_name = "current_global_planner", + switch_service_name = "switch_global_planner"); +``` + +If the parameter is not set or is an empty list, a namespace will be derived from the `default_value` and be loaded as the only plugin available. +``` +global_planner_namespaces: [] +GlobalPlanner: + allow_unknown: true +``` diff --git a/src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md b/src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md new file mode 100755 index 0000000..6d3d5dd --- /dev/null +++ b/src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md @@ -0,0 +1,52 @@ +# nav_2d_utils Polygons and Footprints +This library represents a replacement for [costmap_2d/footprint.h](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/footprint.h) and deals with manipulating polygons. Note that implicitly all polygons here are assumed to be [simple polygons](https://en.wikipedia.org/wiki/Simple_polygon) without "holes." + +## Polygons and the Parameter Server +There have historically been three primary ways to specify a polygon/footprint on the parameter server. The first is to simply specify a radius which is converted to a hexadecagon, i.e. polygon with sixteen sides. This can be read with +``` +nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16); +``` + +The second two ways involve specifying the points of the polygon individually. This can be done with either a string representing a bracketed array of arrays of doubles, `"[[1.0, 2.2], [3.3, 4.2], ...]"`. This can be read with + +``` +nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string); +``` + +Alternatively, with ROS, you can read the points directly from the parameter server in the form of an `XmlRpcValue`, which should be an array of arrays of doubles, which is read with + +``` +nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc); +``` + +If the `XmlRpcValue` is a string, it will call the `polygonFromString` method. + +The above are the traditional methods that were supported by the original `costmap_2d` code. However, we add a fourth method that requires two parallel arrays of x and y coordinates. + +``` +nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, const std::vector& ys); +``` + +All of the above methods (except the radius one) can be loaded as appropriate from the parameter server with +``` +nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name, + bool search = true); +``` +to include the radius, use the logic in `footprint.h` which either uses "footprint" or "robot_radius" +``` +nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true); +``` + +Polygons can be written to parameters with +``` +void polygontoParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name, + bool array_of_arrays = true); +``` + +## Polygon Operations +There are also a handful of methods for examining/manipulating polygons + * `equals` - check if two polygons are equal + * `movePolygonToPose` - translate and rotate a polygon + * `isInside` - check if a point is inside a polygon + * `calculateMinAndMaxDistances` - Calculate the minimum and maximum distance from the origin of a polygon + * `triangulate` - Decompose a polygon into a set of non-overlapping triangles using an open source implementation of the [earcut algorithm](https://github.com/mapbox/earcut.hpp) diff --git a/src/Libraries/nav_2d_utils/include/mapbox/LICENSE b/src/Libraries/nav_2d_utils/include/mapbox/LICENSE new file mode 100755 index 0000000..8bafb57 --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/mapbox/LICENSE @@ -0,0 +1,15 @@ +ISC License + +Copyright (c) 2015, Mapbox + +Permission to use, copy, modify, and/or distribute this software for any purpose +with or without fee is hereby granted, provided that the above copyright notice +and this permission notice appear in all copies. + +THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH +REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, +INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS +OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER +TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF +THIS SOFTWARE. diff --git a/src/Libraries/nav_2d_utils/include/mapbox/NOTES b/src/Libraries/nav_2d_utils/include/mapbox/NOTES new file mode 100755 index 0000000..5c1e99a --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/mapbox/NOTES @@ -0,0 +1,2 @@ +A C++ port of earcut.js, a fast, header-only polygon triangulation library. +https://github.com/mapbox/earcut.hpp diff --git a/src/Libraries/nav_2d_utils/include/mapbox/earcut.hpp b/src/Libraries/nav_2d_utils/include/mapbox/earcut.hpp new file mode 100755 index 0000000..68319c1 --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/mapbox/earcut.hpp @@ -0,0 +1,776 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace mapbox { + +namespace util { + +template struct nth { + inline static typename std::tuple_element::type + get(const T& t) { return std::get(t); }; +}; + +} + +namespace detail { + +template +class Earcut { +public: + std::vector indices; + std::size_t vertices = 0; + + template + void operator()(const Polygon& points); + +private: + struct Node { + Node(N index, double x_, double y_) : i(index), x(x_), y(y_) {} + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + Node(Node&&) = delete; + Node& operator=(Node&&) = delete; + + const N i; + const double x; + const double y; + + // previous and next vertice nodes in a polygon ring + Node* prev = nullptr; + Node* next = nullptr; + + // z-order curve value + int32_t z = 0; + + // previous and next nodes in z-order + Node* prevZ = nullptr; + Node* nextZ = nullptr; + + // indicates whether this is a steiner point + bool steiner = false; + }; + + template Node* linkedList(const Ring& points, const bool clockwise); + Node* filterPoints(Node* start, Node* end = nullptr); + void earcutLinked(Node* ear, int pass = 0); + bool isEar(Node* ear); + bool isEarHashed(Node* ear); + Node* cureLocalIntersections(Node* start); + void splitEarcut(Node* start); + template Node* eliminateHoles(const Polygon& points, Node* outerNode); + void eliminateHole(Node* hole, Node* outerNode); + Node* findHoleBridge(Node* hole, Node* outerNode); + void indexCurve(Node* start); + Node* sortLinked(Node* list); + int32_t zOrder(const double x_, const double y_); + Node* getLeftmost(Node* start); + bool pointInTriangle(double ax, double ay, double bx, double by, double cx, double cy, double px, double py) const; + bool isValidDiagonal(Node* a, Node* b); + double area(const Node* p, const Node* q, const Node* r) const; + bool equals(const Node* p1, const Node* p2); + bool intersects(const Node* p1, const Node* q1, const Node* p2, const Node* q2); + bool intersectsPolygon(const Node* a, const Node* b); + bool locallyInside(const Node* a, const Node* b); + bool middleInside(const Node* a, const Node* b); + Node* splitPolygon(Node* a, Node* b); + template Node* insertNode(std::size_t i, const Point& p, Node* last); + void removeNode(Node* p); + + bool hashing; + double minX, maxX; + double minY, maxY; + double inv_size = 0; + + template > + class ObjectPool { + public: + ObjectPool() { } + ObjectPool(std::size_t blockSize_) { + reset(blockSize_); + } + ~ObjectPool() { + clear(); + } + template + T* construct(Args&&... args) { + if (currentIndex >= blockSize) { + currentBlock = alloc_traits::allocate(alloc, blockSize); + allocations.emplace_back(currentBlock); + currentIndex = 0; + } + T* object = ¤tBlock[currentIndex++]; + alloc_traits::construct(alloc, object, std::forward(args)...); + return object; + } + void reset(std::size_t newBlockSize) { + for (auto allocation : allocations) { + alloc_traits::deallocate(alloc, allocation, blockSize); + } + allocations.clear(); + blockSize = std::max(1, newBlockSize); + currentBlock = nullptr; + currentIndex = blockSize; + } + void clear() { reset(blockSize); } + private: + T* currentBlock = nullptr; + std::size_t currentIndex = 1; + std::size_t blockSize = 1; + std::vector allocations; + Alloc alloc; + typedef typename std::allocator_traits alloc_traits; + }; + ObjectPool nodes; +}; + +template template +void Earcut::operator()(const Polygon& points) { + // reset + indices.clear(); + vertices = 0; + + if (points.empty()) return; + + double x; + double y; + int threshold = 80; + std::size_t len = 0; + + for (size_t i = 0; threshold >= 0 && i < points.size(); i++) { + threshold -= static_cast(points[i].size()); + len += points[i].size(); + } + + //estimate size of nodes and indices + nodes.reset(len * 3 / 2); + indices.reserve(len + points[0].size()); + + Node* outerNode = linkedList(points[0], true); + if (!outerNode || outerNode->prev == outerNode->next) return; + + if (points.size() > 1) outerNode = eliminateHoles(points, outerNode); + + // if the shape is not too simple, we'll use z-order curve hash later; calculate polygon bbox + hashing = threshold < 0; + if (hashing) { + Node* p = outerNode->next; + minX = maxX = outerNode->x; + minY = maxY = outerNode->y; + do { + x = p->x; + y = p->y; + minX = std::min(minX, x); + minY = std::min(minY, y); + maxX = std::max(maxX, x); + maxY = std::max(maxY, y); + p = p->next; + } while (p != outerNode); + + // minX, minY and size are later used to transform coords into integers for z-order calculation + inv_size = std::max(maxX - minX, maxY - minY); + inv_size = inv_size != .0 ? (1. / inv_size) : .0; + } + + earcutLinked(outerNode); + + nodes.clear(); +} + +// create a circular doubly linked list from polygon points in the specified winding order +template template +typename Earcut::Node* +Earcut::linkedList(const Ring& points, const bool clockwise) { + using Point = typename Ring::value_type; + double sum = 0; + const std::size_t len = points.size(); + std::size_t i, j; + Node* last = nullptr; + + // calculate original winding order of a polygon ring + for (i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) { + const auto& p1 = points[i]; + const auto& p2 = points[j]; + const double p20 = util::nth<0, Point>::get(p2); + const double p10 = util::nth<0, Point>::get(p1); + const double p11 = util::nth<1, Point>::get(p1); + const double p21 = util::nth<1, Point>::get(p2); + sum += (p20 - p10) * (p11 + p21); + } + + // link points into circular doubly-linked list in the specified winding order + if (clockwise == (sum > 0)) { + for (i = 0; i < len; i++) last = insertNode(vertices + i, points[i], last); + } else { + for (i = len; i-- > 0;) last = insertNode(vertices + i, points[i], last); + } + + if (last && equals(last, last->next)) { + removeNode(last); + last = last->next; + } + + vertices += len; + + return last; +} + +// eliminate colinear or duplicate points +template +typename Earcut::Node* +Earcut::filterPoints(Node* start, Node* end) { + if (!end) end = start; + + Node* p = start; + bool again; + do { + again = false; + + if (!p->steiner && (equals(p, p->next) || area(p->prev, p, p->next) == 0)) { + removeNode(p); + p = end = p->prev; + + if (p == p->next) break; + again = true; + + } else { + p = p->next; + } + } while (again || p != end); + + return end; +} + +// main ear slicing loop which triangulates a polygon (given as a linked list) +template +void Earcut::earcutLinked(Node* ear, int pass) { + if (!ear) return; + + // interlink polygon nodes in z-order + if (!pass && hashing) indexCurve(ear); + + Node* stop = ear; + Node* prev; + Node* next; + + int iterations = 0; + + // iterate through ears, slicing them one by one + while (ear->prev != ear->next) { + iterations++; + prev = ear->prev; + next = ear->next; + + if (hashing ? isEarHashed(ear) : isEar(ear)) { + // cut off the triangle + indices.emplace_back(prev->i); + indices.emplace_back(ear->i); + indices.emplace_back(next->i); + + removeNode(ear); + + // skipping the next vertice leads to less sliver triangles + ear = next->next; + stop = next->next; + + continue; + } + + ear = next; + + // if we looped through the whole remaining polygon and can't find any more ears + if (ear == stop) { + // try filtering points and slicing again + if (!pass) earcutLinked(filterPoints(ear), 1); + + // if this didn't work, try curing all small self-intersections locally + else if (pass == 1) { + ear = cureLocalIntersections(ear); + earcutLinked(ear, 2); + + // as a last resort, try splitting the remaining polygon into two + } else if (pass == 2) splitEarcut(ear); + + break; + } + } +} + +// check whether a polygon node forms a valid ear with adjacent nodes +template +bool Earcut::isEar(Node* ear) { + const Node* a = ear->prev; + const Node* b = ear; + const Node* c = ear->next; + + if (area(a, b, c) >= 0) return false; // reflex, can't be an ear + + // now make sure we don't have other points inside the potential ear + Node* p = ear->next->next; + + while (p != ear->prev) { + if (pointInTriangle(a->x, a->y, b->x, b->y, c->x, c->y, p->x, p->y) && + area(p->prev, p, p->next) >= 0) return false; + p = p->next; + } + + return true; +} + +template +bool Earcut::isEarHashed(Node* ear) { + const Node* a = ear->prev; + const Node* b = ear; + const Node* c = ear->next; + + if (area(a, b, c) >= 0) return false; // reflex, can't be an ear + + // triangle bbox; min & max are calculated like this for speed + const double minTX = std::min(a->x, std::min(b->x, c->x)); + const double minTY = std::min(a->y, std::min(b->y, c->y)); + const double maxTX = std::max(a->x, std::max(b->x, c->x)); + const double maxTY = std::max(a->y, std::max(b->y, c->y)); + + // z-order range for the current triangle bbox; + const int32_t minZ = zOrder(minTX, minTY); + const int32_t maxZ = zOrder(maxTX, maxTY); + + // first look for points inside the triangle in increasing z-order + Node* p = ear->nextZ; + + while (p && p->z <= maxZ) { + if (p != ear->prev && p != ear->next && + pointInTriangle(a->x, a->y, b->x, b->y, c->x, c->y, p->x, p->y) && + area(p->prev, p, p->next) >= 0) return false; + p = p->nextZ; + } + + // then look for points in decreasing z-order + p = ear->prevZ; + + while (p && p->z >= minZ) { + if (p != ear->prev && p != ear->next && + pointInTriangle(a->x, a->y, b->x, b->y, c->x, c->y, p->x, p->y) && + area(p->prev, p, p->next) >= 0) return false; + p = p->prevZ; + } + + return true; +} + +// go through all polygon nodes and cure small local self-intersections +template +typename Earcut::Node* +Earcut::cureLocalIntersections(Node* start) { + Node* p = start; + do { + Node* a = p->prev; + Node* b = p->next->next; + + // a self-intersection where edge (v[i-1],v[i]) intersects (v[i+1],v[i+2]) + if (!equals(a, b) && intersects(a, p, p->next, b) && locallyInside(a, b) && locallyInside(b, a)) { + indices.emplace_back(a->i); + indices.emplace_back(p->i); + indices.emplace_back(b->i); + + // remove two nodes involved + removeNode(p); + removeNode(p->next); + + p = start = b; + } + p = p->next; + } while (p != start); + + return p; +} + +// try splitting polygon into two and triangulate them independently +template +void Earcut::splitEarcut(Node* start) { + // look for a valid diagonal that divides the polygon into two + Node* a = start; + do { + Node* b = a->next->next; + while (b != a->prev) { + if (a->i != b->i && isValidDiagonal(a, b)) { + // split the polygon in two by the diagonal + Node* c = splitPolygon(a, b); + + // filter colinear points around the cuts + a = filterPoints(a, a->next); + c = filterPoints(c, c->next); + + // run earcut on each half + earcutLinked(a); + earcutLinked(c); + return; + } + b = b->next; + } + a = a->next; + } while (a != start); +} + +// link every hole into the outer loop, producing a single-ring polygon without holes +template template +typename Earcut::Node* +Earcut::eliminateHoles(const Polygon& points, Node* outerNode) { + const size_t len = points.size(); + + std::vector queue; + for (size_t i = 1; i < len; i++) { + Node* list = linkedList(points[i], false); + if (list) { + if (list == list->next) list->steiner = true; + queue.push_back(getLeftmost(list)); + } + } + std::sort(queue.begin(), queue.end(), [](const Node* a, const Node* b) { + return a->x < b->x; + }); + + // process holes from left to right + for (size_t i = 0; i < queue.size(); i++) { + eliminateHole(queue[i], outerNode); + outerNode = filterPoints(outerNode, outerNode->next); + } + + return outerNode; +} + +// find a bridge between vertices that connects hole with an outer ring and and link it +template +void Earcut::eliminateHole(Node* hole, Node* outerNode) { + outerNode = findHoleBridge(hole, outerNode); + if (outerNode) { + Node* b = splitPolygon(outerNode, hole); + filterPoints(b, b->next); + } +} + +// David Eberly's algorithm for finding a bridge between hole and outer polygon +template +typename Earcut::Node* +Earcut::findHoleBridge(Node* hole, Node* outerNode) { + Node* p = outerNode; + double hx = hole->x; + double hy = hole->y; + double qx = -std::numeric_limits::infinity(); + Node* m = nullptr; + + // find a segment intersected by a ray from the hole's leftmost Vertex to the left; + // segment's endpoint with lesser x will be potential connection Vertex + do { + if (hy <= p->y && hy >= p->next->y && p->next->y != p->y) { + double x = p->x + (hy - p->y) * (p->next->x - p->x) / (p->next->y - p->y); + if (x <= hx && x > qx) { + qx = x; + if (x == hx) { + if (hy == p->y) return p; + if (hy == p->next->y) return p->next; + } + m = p->x < p->next->x ? p : p->next; + } + } + p = p->next; + } while (p != outerNode); + + if (!m) return 0; + + if (hx == qx) return m->prev; + + // look for points inside the triangle of hole Vertex, segment intersection and endpoint; + // if there are no points found, we have a valid connection; + // otherwise choose the Vertex of the minimum angle with the ray as connection Vertex + + const Node* stop = m; + double tanMin = std::numeric_limits::infinity(); + double tanCur = 0; + + p = m->next; + double mx = m->x; + double my = m->y; + + while (p != stop) { + if (hx >= p->x && p->x >= mx && hx != p->x && + pointInTriangle(hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, p->x, p->y)) { + + tanCur = std::abs(hy - p->y) / (hx - p->x); // tangential + + if ((tanCur < tanMin || (tanCur == tanMin && p->x > m->x)) && locallyInside(p, hole)) { + m = p; + tanMin = tanCur; + } + } + + p = p->next; + } + + return m; +} + +// interlink polygon nodes in z-order +template +void Earcut::indexCurve(Node* start) { + assert(start); + Node* p = start; + + do { + p->z = p->z ? p->z : zOrder(p->x, p->y); + p->prevZ = p->prev; + p->nextZ = p->next; + p = p->next; + } while (p != start); + + p->prevZ->nextZ = nullptr; + p->prevZ = nullptr; + + sortLinked(p); +} + +// Simon Tatham's linked list merge sort algorithm +// http://www.chiark.greenend.org.uk/~sgtatham/algorithms/listsort.html +template +typename Earcut::Node* +Earcut::sortLinked(Node* list) { + assert(list); + Node* p; + Node* q; + Node* e; + Node* tail; + int i, numMerges, pSize, qSize; + int inSize = 1; + + for (;;) { + p = list; + list = nullptr; + tail = nullptr; + numMerges = 0; + + while (p) { + numMerges++; + q = p; + pSize = 0; + for (i = 0; i < inSize; i++) { + pSize++; + q = q->nextZ; + if (!q) break; + } + + qSize = inSize; + + while (pSize > 0 || (qSize > 0 && q)) { + + if (pSize == 0) { + e = q; + q = q->nextZ; + qSize--; + } else if (qSize == 0 || !q) { + e = p; + p = p->nextZ; + pSize--; + } else if (p->z <= q->z) { + e = p; + p = p->nextZ; + pSize--; + } else { + e = q; + q = q->nextZ; + qSize--; + } + + if (tail) tail->nextZ = e; + else list = e; + + e->prevZ = tail; + tail = e; + } + + p = q; + } + + tail->nextZ = nullptr; + + if (numMerges <= 1) return list; + + inSize *= 2; + } +} + +// z-order of a Vertex given coords and size of the data bounding box +template +int32_t Earcut::zOrder(const double x_, const double y_) { + // coords are transformed into non-negative 15-bit integer range + int32_t x = static_cast(32767.0 * (x_ - minX) * inv_size); + int32_t y = static_cast(32767.0 * (y_ - minY) * inv_size); + + x = (x | (x << 8)) & 0x00FF00FF; + x = (x | (x << 4)) & 0x0F0F0F0F; + x = (x | (x << 2)) & 0x33333333; + x = (x | (x << 1)) & 0x55555555; + + y = (y | (y << 8)) & 0x00FF00FF; + y = (y | (y << 4)) & 0x0F0F0F0F; + y = (y | (y << 2)) & 0x33333333; + y = (y | (y << 1)) & 0x55555555; + + return x | (y << 1); +} + +// find the leftmost node of a polygon ring +template +typename Earcut::Node* +Earcut::getLeftmost(Node* start) { + Node* p = start; + Node* leftmost = start; + do { + if (p->x < leftmost->x || (p->x == leftmost->x && p->y < leftmost->y)) + leftmost = p; + p = p->next; + } while (p != start); + + return leftmost; +} + +// check if a point lies within a convex triangle +template +bool Earcut::pointInTriangle(double ax, double ay, double bx, double by, double cx, double cy, double px, double py) const { + return (cx - px) * (ay - py) - (ax - px) * (cy - py) >= 0 && + (ax - px) * (by - py) - (bx - px) * (ay - py) >= 0 && + (bx - px) * (cy - py) - (cx - px) * (by - py) >= 0; +} + +// check if a diagonal between two polygon nodes is valid (lies in polygon interior) +template +bool Earcut::isValidDiagonal(Node* a, Node* b) { + return a->next->i != b->i && a->prev->i != b->i && !intersectsPolygon(a, b) && + locallyInside(a, b) && locallyInside(b, a) && middleInside(a, b); +} + +// signed area of a triangle +template +double Earcut::area(const Node* p, const Node* q, const Node* r) const { + return (q->y - p->y) * (r->x - q->x) - (q->x - p->x) * (r->y - q->y); +} + +// check if two points are equal +template +bool Earcut::equals(const Node* p1, const Node* p2) { + return p1->x == p2->x && p1->y == p2->y; +} + +// check if two segments intersect +template +bool Earcut::intersects(const Node* p1, const Node* q1, const Node* p2, const Node* q2) { + if ((equals(p1, q1) && equals(p2, q2)) || + (equals(p1, q2) && equals(p2, q1))) return true; + return (area(p1, q1, p2) > 0) != (area(p1, q1, q2) > 0) && + (area(p2, q2, p1) > 0) != (area(p2, q2, q1) > 0); +} + +// check if a polygon diagonal intersects any polygon segments +template +bool Earcut::intersectsPolygon(const Node* a, const Node* b) { + const Node* p = a; + do { + if (p->i != a->i && p->next->i != a->i && p->i != b->i && p->next->i != b->i && + intersects(p, p->next, a, b)) return true; + p = p->next; + } while (p != a); + + return false; +} + +// check if a polygon diagonal is locally inside the polygon +template +bool Earcut::locallyInside(const Node* a, const Node* b) { + return area(a->prev, a, a->next) < 0 ? + area(a, b, a->next) >= 0 && area(a, a->prev, b) >= 0 : + area(a, b, a->prev) < 0 || area(a, a->next, b) < 0; +} + +// check if the middle Vertex of a polygon diagonal is inside the polygon +template +bool Earcut::middleInside(const Node* a, const Node* b) { + const Node* p = a; + bool inside = false; + double px = (a->x + b->x) / 2; + double py = (a->y + b->y) / 2; + do { + if (((p->y > py) != (p->next->y > py)) && p->next->y != p->y && + (px < (p->next->x - p->x) * (py - p->y) / (p->next->y - p->y) + p->x)) + inside = !inside; + p = p->next; + } while (p != a); + + return inside; +} + +// link two polygon vertices with a bridge; if the vertices belong to the same ring, it splits +// polygon into two; if one belongs to the outer ring and another to a hole, it merges it into a +// single ring +template +typename Earcut::Node* +Earcut::splitPolygon(Node* a, Node* b) { + Node* a2 = nodes.construct(a->i, a->x, a->y); + Node* b2 = nodes.construct(b->i, b->x, b->y); + Node* an = a->next; + Node* bp = b->prev; + + a->next = b; + b->prev = a; + + a2->next = an; + an->prev = a2; + + b2->next = a2; + a2->prev = b2; + + bp->next = b2; + b2->prev = bp; + + return b2; +} + +// create a node and util::optionally link it with previous one (in a circular doubly linked list) +template template +typename Earcut::Node* +Earcut::insertNode(std::size_t i, const Point& pt, Node* last) { + Node* p = nodes.construct(static_cast(i), util::nth<0, Point>::get(pt), util::nth<1, Point>::get(pt)); + + if (!last) { + p->prev = p; + p->next = p; + + } else { + assert(last); + p->next = last->next; + p->prev = last; + last->next->prev = p; + last->next = p; + } + return p; +} + +template +void Earcut::removeNode(Node* p) { + p->next->prev = p->prev; + p->prev->next = p->next; + + if (p->prevZ) p->prevZ->nextZ = p->nextZ; + if (p->nextZ) p->nextZ->prevZ = p->prevZ; +} +} + +template +std::vector earcut(const Polygon& poly) { + mapbox::detail::Earcut earcut; + earcut(poly); + return std::move(earcut.indices); +} +} diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/bounds.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/bounds.h new file mode 100755 index 0000000..6c95c26 --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/bounds.h @@ -0,0 +1,91 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * 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 NAV_2D_UTILS_BOUNDS_H +#define NAV_2D_UTILS_BOUNDS_H + +#include +#include +#include + +/** + * @brief A set of utility functions for Bounds objects interacting with other messages/types + */ +namespace nav_2d_utils +{ + +/** + * @brief return a floating point bounds that covers the entire NavGrid + * @param info Info for the NavGrid + * @return bounds covering the entire NavGrid + */ +nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo& info); + +/** + * @brief return an integral bounds that covers the entire NavGrid + * @param info Info for the NavGrid + * @return bounds covering the entire NavGrid + */ +nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo& info); + +/** + * @brief Translate real-valued bounds to uint coordinates based on nav_grid info + * @param info Information used when converting the coordinates + * @param bounds floating point bounds object + * @returns corresponding UIntBounds for parameter + */ +nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::Bounds& bounds); + +/** + * @brief Translate uint bounds to real-valued coordinates based on nav_grid info + * @param info Information used when converting the coordinates + * @param bounds UIntBounds object + * @returns corresponding floating point bounds for parameter + */ +nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::UIntBounds& bounds); + +/** + * @brief divide the given bounds up into sub-bounds of roughly equal size + * @param original_bounds The original bounds to divide + * @param n_cols Positive number of columns to divide the bounds into + * @param n_rows Positive number of rows to divide the bounds into + * @return vector of a maximum of n_cols*n_rows nonempty bounds + * @throws std::invalid_argument when n_cols or n_rows is zero + */ +std::vector divideBounds(const nav_core2::UIntBounds& original_bounds, + unsigned int n_cols, unsigned int n_rows); + +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_BOUNDS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/conversions.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/conversions.h new file mode 100755 index 0000000..ce0e66f --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/conversions.h @@ -0,0 +1,106 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_2D_UTILS_CONVERSIONS_H +#define NAV_2D_UTILS_CONVERSIONS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// #include +#include +#include + +namespace nav_2d_utils +{ +// Twist Transformations +geometry_msgs::Twist twist2Dto3D(const nav_2d_msgs::Twist2D& cmd_vel_2d); +nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist& cmd_vel); + +// Point Transformations +nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point& point); +nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32& point); +geometry_msgs::Point pointToPoint3D(const nav_2d_msgs::Point2D& point); +geometry_msgs::Point32 pointToPoint32(const nav_2d_msgs::Point2D& point); + +// Pose Transformations +// nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped& pose); +nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped& pose); +geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D& pose2d); +geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped& pose2d); +geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d, + const std::string& frame, const robot::Time& stamp); + +// Path Transformations +nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path& path); +nav_msgs::Path posesToPath(const std::vector& poses); +nav_2d_msgs::Path2D posesToPath2D(const std::vector& poses); +nav_msgs::Path poses2DToPath(const std::vector& poses, + const std::string& frame, const robot::Time& stamp); +nav_msgs::Path pathToPath(const nav_2d_msgs::Path2D& path2d); + +// Polygon Transformations +geometry_msgs::Polygon polygon2Dto3D(const nav_2d_msgs::Polygon2D& polygon_2d); +nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon& polygon_3d); +geometry_msgs::PolygonStamped polygon2Dto3D(const nav_2d_msgs::Polygon2DStamped& polygon_2d); +nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped& polygon_3d); + +// Info Transformations +nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo& info); +nav_grid::NavGridInfo fromMsg(const nav_2d_msgs::NavGridInfo& msg); +geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo& info); +geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo& info); +nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData& metadata, const std::string& frame); +nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo & info); + +// Bounds Transformations +nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds& bounds); +nav_core2::UIntBounds fromMsg(const nav_2d_msgs::UIntBounds& msg); + +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_CONVERSIONS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h new file mode 100755 index 0000000..8f92f24 --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_2D_UTILS_FOOTPRINT_H +#define NAV_2D_UTILS_FOOTPRINT_H + +#include +#include + +namespace nav_2d_utils +{ + +/** + * @brief Load the robot footprint either as a polygon from the footprint parameter or from robot_radius + * + * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter + * is present. + */ +nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true); + +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_FOOTPRINT_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/geometry_help.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/geometry_help.h new file mode 100755 index 0000000..3afae5a --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/geometry_help.h @@ -0,0 +1,85 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_2D_UTILS_GEOMETRY_HELP_H +#define NAV_2D_UTILS_GEOMETRY_HELP_H + +#include + +namespace nav_2d_utils +{ +/** + * @brief Distance from point (pX, pY) to closest point on line from (x0, y0) to (x1, y1) + * @param pX + * @param pY + * @param x0 + * @param y0 + * @param x1 + * @param y1 + * @return shortest distance from point to line + */ +inline double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1) +{ + double A = pX - x0; + double B = pY - y0; + double C = x1 - x0; + double D = y1 - y0; + + double dot = A * C + B * D; + double len_sq = C * C + D * D; + double param = dot / len_sq; + + double xx, yy; + + if (param < 0) + { + xx = x0; + yy = y0; + } + else if (param > 1) + { + xx = x1; + yy = y1; + } + else + { + xx = x0 + param * C; + yy = y0 + param * D; + } + + return hypot(pX - xx, pY - yy); +} +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_GEOMETRY_HELP_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h new file mode 100755 index 0000000..911ffbd --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_2D_UTILS_ODOM_SUBSCRIBER_H +#define NAV_2D_UTILS_ODOM_SUBSCRIBER_H + +#include +#include +#include +#include +#include +#include +#include + +namespace nav_2d_utils +{ + +/** + * @class OdomSubscriber + * Wrapper for some common odometry operations. Subscribes to the topic with a mutex. + */ +class OdomSubscriber +{ +public: + /** + * @brief Constructor that subscribes to an Odometry topic + * + * @param nh NodeHandle for creating subscriber + * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set. + */ + explicit OdomSubscriber(YAML::Node& nh, std::string default_topic = "odom") + { + std::string odom_topic; + // nh.param("odom_topic", odom_topic, default_topic); + // odom_sub_ = nh.subscribe(odom_topic, 1, boost::bind(&OdomSubscriber::odomCallback, this, _1)); + } + + inline nav_2d_msgs::Twist2D getTwist() { return odom_vel_.velocity; } + inline nav_2d_msgs::Twist2DStamped getTwistStamped() { return odom_vel_; } + +protected: + void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) + { + std::cout << "odom received!" << std::endl; + boost::mutex::scoped_lock lock(odom_mutex_); + odom_vel_.header = msg->header; + odom_vel_.velocity = twist3Dto2D(msg->twist.twist); + } + + nav_2d_msgs::Twist2DStamped odom_vel_; + boost::mutex odom_mutex_; +}; + +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_ODOM_SUBSCRIBER_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h new file mode 100755 index 0000000..afae27e --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h @@ -0,0 +1,149 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_2D_UTILS_PARAMETERS_H +#define NAV_2D_UTILS_PARAMETERS_H + +#include + +namespace nav_2d_utils +{ + +/** + * @brief Search for a parameter and load it, or use the default value + * + * This templated function shortens a commonly used ROS pattern in which you + * search for a parameter and get its value if it exists, otherwise returning a default value. + * + * @param nh NodeHandle to start the parameter search from + * @param param_name Name of the parameter to search for + * @param default_value Value to return if not found + * @return Value of parameter if found, otherwise the default_value + */ +template +param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, const param_t& default_value) +{ + std::string resolved_name; + // if (nh.searchParam(param_name, resolved_name)) + // { + // param_t value = default_value; + // nh.param(resolved_name, value, default_value); + // return value; + // } + return default_value; +} + +/** + * @brief Load a parameter from one of two namespaces. Complain if it uses the old name. + * @param nh NodeHandle to look for the parameter in + * @param current_name Parameter name that is current, i.e. not deprecated + * @param old_name Deprecated parameter name + * @param default_value If neither parameter is present, return this value + * @return The value of the parameter or the default value + */ +template +param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string current_name, + const std::string old_name, const param_t& default_value) +{ + param_t value; + if (nh[current_name] && nh[current_name].IsDefined()) + { + value = nh[current_name].as(); + return value; + } + if (nh[old_name] && nh[old_name].IsDefined()) + { + std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl; + value = nh[old_name].as(); + return value; + } + return default_value; +} + +/** + * @brief If a deprecated parameter exists, complain and move to its new location + * @param nh NodeHandle to look for the parameter in + * @param current_name Parameter name that is current, i.e. not deprecated + * @param old_name Deprecated parameter name + */ +template +void moveDeprecatedParameter(const YAML::Node& nh, const std::string current_name, const std::string old_name) +{ + if (!nh[old_name] || !nh[old_name].IsDefined()) return; + + param_t value; + std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl; + value = nh[old_name].as(); + nh[current_name] = value; +} + +/** + * @brief Move a parameter from one place to another + * + * For use loading old parameter structures into new places. + * + * If the new name already has a value, don't move the old value there. + * + * @param nh NodeHandle for loading/saving params + * @param old_name Parameter name to move/remove + * @param current_name Destination parameter name + * @param default_value Value to save in the new name if old parameter is not found. + * @param should_delete If true, whether to delete the parameter from the old name + */ +template +void moveParameter(const YAML::Node& nh, std::string old_name, + std::string current_name, param_t default_value, bool should_delete = true) +{ + // if (nh.hasParam(current_name)) + // { + // if (should_delete) + // nh.deleteParam(old_name); + // return; + // } + // XmlRpc::XmlRpcValue value; + // if (nh.hasParam(old_name)) + // { + // nh.getParam(old_name, value); + // if (should_delete) nh.deleteParam(old_name); + // } + // else + // value = default_value; + + // nh.setParam(current_name, value); +} + + +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_PARAMETERS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/path_ops.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/path_ops.h new file mode 100755 index 0000000..187b762 --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/path_ops.h @@ -0,0 +1,88 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_2D_UTILS_PATH_OPS_H +#define NAV_2D_UTILS_PATH_OPS_H + +#include + +namespace nav_2d_utils +{ +/** + * @brief Calculate the linear distance between two poses + */ +double poseDistance(const geometry_msgs::Pose2D& pose0, const geometry_msgs::Pose2D& pose1); + +/** + * @brief Calculate the length of the plan, starting from the given index + */ +double getPlanLength(const nav_2d_msgs::Path2D& plan, const unsigned int start_index = 0); + +/** + * @brief Calculate the length of the plan from the pose on the plan closest to the given pose + */ +double getPlanLength(const nav_2d_msgs::Path2D& plan, const geometry_msgs::Pose2D& query_pose); + +/** + * @brief Increase plan resolution to match that of the costmap by adding points linearly between points + * + * @param global_plan_in input plan + * @param resolution desired distance between waypoints + * @return Higher resolution plan + */ +nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D& global_plan_in, double resolution); + +/** + * @brief Decrease the length of the plan by eliminating colinear points + * + * Uses the Ramer Douglas Peucker algorithm. Ignores theta values + * + * @param input_path Path to compress + * @param epsilon maximum allowable error. Increase for greater compression. + * @return Path2D with possibly fewer poses + */ +nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D& input_path, double epsilon = 0.1); + +/** + * @brief Convenience function to add a pose to a path in one line. + * @param path Path to add to + * @param x x-coordinate + * @param y y-coordinate + * @param theta theta (if needed) + */ +void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta = 0.0); + +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_PATH_OPS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h new file mode 100755 index 0000000..5e1b286 --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h @@ -0,0 +1,274 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_2D_UTILS_PLUGIN_MUX_H +#define NAV_2D_UTILS_PLUGIN_MUX_H + +#include +#include +#include +#include +#include +#include +#include + +namespace nav_2d_utils +{ +/** + * @class PluginMux + * @brief An organizer for switching between multiple different plugins of the same type + * + * The different plugins are specified using a list of strings on the parameter server, each of which is a namespace. + * The specific type and additional parameters for each plugin are specified on the parameter server in that namespace. + * All the plugins are loaded initially, but only one is the "current" plugin at any particular time, which is published + * on a latched topic AND stored on the ROS parameter server. You can switch which plugin is current either through a + * C++ or ROS interface. + */ +template +class PluginMux +{ +public: + /** + * @brief Main Constructor - Loads plugin(s) and sets up ROS interfaces + * + * @param plugin_package The package of the plugin type + * @param plugin_class The class name for the plugin type + * @param parameter_name Name of parameter for the namespaces. + * @param default_value If class name is not specified, which plugin should be loaded + * @param ros_name ROS name for setting up topic and parameter + * @param switch_service_name ROS name for setting up the ROS service + */ + PluginMux(const std::string& plugin_package, const std::string& plugin_class, + const std::string& parameter_name, const std::string& default_value, + const std::string& ros_name = "current_plugin", const std::string& switch_service_name = "switch_plugin"); + + /** + * @brief Create an instance of the given plugin_class_name and save it with the given plugin_name + * @param plugin_name Namespace for the new plugin + * @param plugin_class_name Class type name for the new plugin + */ + void addPlugin(const std::string& plugin_name, const std::string& plugin_class_name); + + /** + * @brief C++ Interface for switching to a given plugin + * + * @param name Namespace to set current plugin to + * @return true if that namespace exists and is loaded properly + */ + bool usePlugin(const std::string& name) + { + // If plugin with given name doesn't exist, return false + if (plugins_.count(name) == 0) return false; + + if (switch_callback_) + { + switch_callback_(current_plugin_, name); + } + + // Switch Mux + current_plugin_ = name; + + // Update ROS info + std_msgs::String str_msg; + str_msg.data = current_plugin_; + current_plugin_pub_.publish(str_msg); + private_nh_.setParam(ros_name_, current_plugin_); + + return true; + } + + /** + * @brief Get the Current Plugin Name + * @return Name of the current plugin + */ + std::string getCurrentPluginName() const + { + return current_plugin_; + } + + /** + * @brief Check to see if a plugin exists + * @param name Namespace to set current plugin to + * @return True if the plugin exists + */ + bool hasPlugin(const std::string& name) const + { + return plugins_.find(name) != plugins_.end(); + } + + /** + * @brief Get the Specified Plugin + * @param name Name of plugin to get + * @return Reference to specified plugin + */ + T& getPlugin(const std::string& name) + { + if (!hasPlugin(name)) + throw std::invalid_argument("Plugin not found"); + return *plugins_[name]; + } + + /** + * @brief Get the Current Plugin + * @return Reference to current plugin + */ + T& getCurrentPlugin() + { + return getPlugin(current_plugin_); + } + + /** + * @brief Get the current list of plugin names + */ + std::vector getPluginNames() const + { + std::vector names; + for (auto& kv : plugins_) + { + names.push_back(kv.first); + } + return names; + } + + /** + * @brief alias for the function-type of the callback fired when the plugin switches. + * + * The first parameter will be the namespace of the plugin that was previously used. + * The second parameter will be the namespace of the plugin that is being switched to. + */ + using SwitchCallback = std::function; + + /** + * @brief Set the callback fired when the plugin switches + * + * In addition to switching which plugin is being used via directly calling `usePlugin` + * a switch can also be triggered externally via ROS service. In such a case, other classes + * may want to know when this happens. This is accomplished using the SwitchCallback, which + * will be called regardless of how the plugin is switched. + */ + void setSwitchCallback(SwitchCallback switch_callback) { switch_callback_ = switch_callback; } + +protected: + /** + * @brief ROS Interface for Switching Plugins + */ + bool switchPluginService(nav_2d_msgs::SwitchPlugin::Request &req, nav_2d_msgs::SwitchPlugin::Response &resp) + { + std::string name = req.new_plugin; + if (usePlugin(name)) + { + resp.success = true; + resp.message = "Loaded plugin namespace " + name + "."; + } + else + { + resp.success = false; + resp.message = "Namespace " + name + " not configured!"; + } + return true; + } + + // Plugin Management + pluginlib::ClassLoader plugin_loader_; + std::map> plugins_; + std::string current_plugin_; + + // ROS Interface + ros::ServiceServer switch_plugin_srv_; + ros::Publisher current_plugin_pub_; + YAML::Node private_nh_; + std::string ros_name_; + + // Switch Callback + SwitchCallback switch_callback_; +}; + +// ********************************************************************************************************************* +// ****************** Implementation of Templated Methods ************************************************************** +// ********************************************************************************************************************* +template +PluginMux::PluginMux(const std::string& plugin_package, const std::string& plugin_class, + const std::string& parameter_name, const std::string& default_value, + const std::string& ros_name, const std::string& switch_service_name) + : plugin_loader_(plugin_package, plugin_class), private_nh_("~"), ros_name_(ros_name), switch_callback_(nullptr) +{ + // Create the latched publisher + current_plugin_pub_ = private_nh_.advertise(ros_name_, 1, true); + + // Load Plugins + std::string plugin_class_name; + std::vector plugin_namespaces; + private_nh_.getParam(parameter_name, plugin_namespaces); + if (plugin_namespaces.size() == 0) + { + // If no namespaces are listed, use the name of the default class as the singular namespace. + std::string plugin_name = plugin_loader_.getName(default_value); + plugin_namespaces.push_back(plugin_name); + } + + for (const std::string& the_namespace : plugin_namespaces) + { + // Load the class name from namespace/plugin_class, or use default value + private_nh_.param(std::string(the_namespace + "/plugin_class"), plugin_class_name, default_value); + addPlugin(the_namespace, plugin_class_name); + } + + // By default, use the first one as current + usePlugin(plugin_namespaces[0]); + + // Now that the plugins are created, advertise the service if there are multiple + if (plugin_namespaces.size() > 1) + { + switch_plugin_srv_ = private_nh_.advertiseService(switch_service_name, &PluginMux::switchPluginService, this); + } +} + +template +void PluginMux::addPlugin(const std::string& plugin_name, const std::string& plugin_class_name) +{ + try + { + plugins_[plugin_name] = plugin_loader_.createInstance(plugin_class_name); + } + catch (const pluginlib::PluginlibException& ex) + { + ROS_FATAL_NAMED("PluginMux", + "Failed to load the plugin: %s. Exception: %s", plugin_name.c_str(), ex.what()); + exit(EXIT_FAILURE); + } +} + +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_PLUGIN_MUX_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h new file mode 100755 index 0000000..d7f6fde --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h @@ -0,0 +1,199 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_2D_UTILS_POLYGONS_H +#define NAV_2D_UTILS_POLYGONS_H + +#include +#include +#include +#include +#include +#include + +namespace nav_2d_utils +{ + +/** + * @class PolygonParseException + * @brief Exception to throw when Polygon doesn't load correctly + */ +class PolygonParseException: public std::runtime_error +{ +public: + explicit PolygonParseException(const std::string& description) : std::runtime_error(description) {} +}; + +/** + * @brief Parse a vector of vectors of doubles from a string. + * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] + * + * @param input The string to parse + * @return a vector of vectors of doubles + */ +std::vector > parseVVD(const std::string& input); + +/** + * @brief Create a "circular" polygon from a given radius + * + * @param radius Radius of the polygon + * @param num_points Number of points in the resulting polygon + * @return A rotationally symmetric polygon with the specified number of vertices + */ +nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16); + +/** + * @brief Make a polygon from the given string. + * Format should be bracketed array of arrays of doubles, like so: [[1.0, 2.2], [3.3, 4.2], ...] + * + * @param polygon_string The string to parse + * @return Polygon2D + */ +nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string); + +// /** +// * @brief Load a polygon from a parameter, whether it is a string or array, or two arrays +// * @param nh Node handle to load parameter from +// * @param parameter_name Name of the parameter +// * @param search Whether to search up the namespace for the parameter name +// * @return Loaded polygon +// */ +// nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name, +// bool search = true); + +/** + * @brief Create a polygon from the given XmlRpcValue. + * + * @param polygon_xmlrpc should be an array of arrays, where the top-level array should have + * 3 or more elements, and the sub-arrays should all have exactly 2 elements + * (x and y coordinates). + */ +nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc); + +/** + * @brief Create XMLRPC Value for writing the polygon to the parameter server + * @param polygon Polygon to convert + * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays + * @return XmlRpcValue + */ +XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true); + +// /** +// * @brief Save a polygon to a parameter +// * @param polygon The Polygon +// * @param nh Node handle to save the parameter to +// * @param parameter_name Name of the parameter +// * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays +// */ +// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name, +// bool array_of_arrays = true); + +/** + * @brief Create a polygon from two parallel arrays + * + * @param xs Array of doubles representing x coordinates, at least three elements long + * @param ys Array of doubles representing y coordinates, matching length of xs + */ +nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, const std::vector& ys); + +/** + * @brief Create two parallel arrays from a polygon + * + * @param[in] polygon + * @param[out] xs Array of doubles representing x coordinates, to be populated + * @param[out] ys Array of doubles representing y coordinates, to be populated + */ +void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector& xs, std::vector& ys); + +/** + * @brief check if two polygons are equal. Used in testing + */ +bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D& polygon1); + +/** + * @brief Translate and rotate a polygon to a new pose + * @param polygon The polygon + * @param pose The x, y and theta to use when moving the polygon + * @return A new moved polygon + */ +nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon, + const geometry_msgs::Pose2D& pose); + +/** + * @brief Check if a given point is inside of a polygon + * + * Borders are considered outside. + * + * @param polygon Polygon to check + * @param x x coordinate + * @param y y coordinate + * @return true if point is inside polygon + */ +bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const double y); + +/** + * @brief Calculate the minimum and maximum distance from (0, 0) to any point on the polygon + * @param[in] polygon polygon to analyze + * @param[out] min_dist + * @param[out] max_dist + */ +void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist); + +/** + * @brief Decompose a complex polygon into a set of triangles. + * + * See https://en.wikipedia.org/wiki/Polygon_triangulation + * + * Implementation from https://github.com/mapbox/earcut.hpp + * + * @param polygon The complex polygon to deconstruct + * @return A vector of points where each set of three points represents a triangle + */ +std::vector triangulate(const nav_2d_msgs::ComplexPolygon2D& polygon); + +/** + * @brief Decompose a simple polygon into a set of triangles. + * + * See https://en.wikipedia.org/wiki/Polygon_triangulation + * + * Implementation from https://github.com/mapbox/earcut.hpp + * + * @param polygon The polygon to deconstruct + * @return A vector of points where each set of three points represents a triangle + */ +std::vector triangulate(const nav_2d_msgs::Polygon2D& polygon); + +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_POLYGONS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/tf_help.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/tf_help.h new file mode 100755 index 0000000..e17581f --- /dev/null +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/tf_help.h @@ -0,0 +1,91 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_2D_UTILS_TF_HELP_H +#define NAV_2D_UTILS_TF_HELP_H + +#include +#include +#include +#include + +namespace nav_2d_utils +{ +/** + * @brief Transform a PoseStamped from one frame to another while catching exceptions + * + * Also returns immediately if the frames are equal. + * @param tf Smart pointer to TFListener + * @param frame Frame to transform the pose into + * @param in_pose Pose to transform + * @param out_pose Place to store the resulting transformed pose + * @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead. + * @return True if successful transform + */ + bool transformPose(const TFListenerPtr tf, const std::string frame, + const geometry_msgs::PoseStamped& in_pose, geometry_msgs::PoseStamped& out_pose, + const bool extrapolation_fallback = true); + +/** + * @brief Transform a Pose2DStamped from one frame to another while catching exceptions + * + * Also returns immediately if the frames are equal. + * @param tf Smart pointer to TFListener + * @param frame Frame to transform the pose into + * @param in_pose Pose to transform + * @param out_pose Place to store the resulting transformed pose + * @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead. + * @return True if successful transform + */ +bool transformPose(const TFListenerPtr tf, const std::string frame, + const nav_2d_msgs::Pose2DStamped& in_pose, nav_2d_msgs::Pose2DStamped& out_pose, + const bool extrapolation_fallback = true); + +/** + * @brief Transform a Pose2DStamped into another frame + * + * Note that this returns a transformed pose + * regardless of whether the transform was successfully performed. + * + * @param tf Smart pointer to TFListener + * @param pose Pose to transform + * @param frame_id Frame to transform the pose into + * @return The resulting transformed pose + */ +geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped& pose, + const std::string& frame_id); + +} // namespace nav_2d_utils + +#endif // NAV_2D_UTILS_TF_HELP_H diff --git a/src/Libraries/nav_2d_utils/package.xml b/src/Libraries/nav_2d_utils/package.xml new file mode 100755 index 0000000..0d63c96 --- /dev/null +++ b/src/Libraries/nav_2d_utils/package.xml @@ -0,0 +1,26 @@ + + nav_2d_utils + 0.7.10 + + nav_2d_utils is the second generation of the transform library, which lets + the user keep track of multiple coordinate frames over time. nav_2d_utils + maintains the relationship between coordinate frames in a tree + structure buffered in time, and lets the user transform points, + vectors, etc between any two coordinate frames at any desired + point in time. + + Tully Foote + Eitan Marder-Eppstein + Wim Meeussen + Tully Foote + BSD + + http://www.ros.org/wiki/nav_2d_utils + + catkin + + libconsole-bridge-dev + + libconsole-bridge-dev + + \ No newline at end of file diff --git a/src/Libraries/nav_2d_utils/setup.py b/src/Libraries/nav_2d_utils/setup.py new file mode 100755 index 0000000..77852f4 --- /dev/null +++ b/src/Libraries/nav_2d_utils/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +package_info = generate_distutils_setup( + packages=['nav_2d_utils'], + package_dir={'': 'src'} +) + +setup(**package_info) diff --git a/src/Libraries/nav_2d_utils/src/bounds.cpp b/src/Libraries/nav_2d_utils/src/bounds.cpp new file mode 100755 index 0000000..4a0ea72 --- /dev/null +++ b/src/Libraries/nav_2d_utils/src/bounds.cpp @@ -0,0 +1,102 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * 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 nav_2d_utils +{ +nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo& info) +{ + return nav_core2::Bounds(info.origin_x, info.origin_y, + info.origin_x + info.resolution * info.width, info.origin_y + info.resolution * info.height); +} + +nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo& info) +{ + // bounds are inclusive, so we subtract one + return nav_core2::UIntBounds(0, 0, info.width - 1, info.height - 1); +} + +nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::Bounds& bounds) +{ + unsigned int g_min_x, g_min_y, g_max_x, g_max_y; + worldToGridBounded(info, bounds.getMinX(), bounds.getMinY(), g_min_x, g_min_y); + worldToGridBounded(info, bounds.getMaxX(), bounds.getMaxY(), g_max_x, g_max_y); + return nav_core2::UIntBounds(g_min_x, g_min_y, g_max_x, g_max_y); +} + +nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::UIntBounds& bounds) +{ + double min_x, min_y, max_x, max_y; + gridToWorld(info, bounds.getMinX(), bounds.getMinY(), min_x, min_y); + gridToWorld(info, bounds.getMaxX(), bounds.getMaxY(), max_x, max_y); + return nav_core2::Bounds(min_x, min_y, max_x, max_y); +} + +std::vector divideBounds(const nav_core2::UIntBounds& original_bounds, + unsigned int n_cols, unsigned int n_rows) +{ + if (n_cols * n_rows == 0) + { + throw std::invalid_argument("Number of rows and columns must be positive (not zero)"); + } + unsigned int full_width = original_bounds.getWidth(), + full_height = original_bounds.getHeight(); + + unsigned int small_width = static_cast(ceil(static_cast(full_width) / n_cols)), + small_height = static_cast(ceil(static_cast(full_height) / n_rows)); + + std::vector divided; + + for (unsigned int row = 0; row < n_rows; row++) + { + unsigned int min_y = original_bounds.getMinY() + small_height * row; + unsigned int max_y = std::min(min_y + small_height - 1, original_bounds.getMaxY()); + + for (unsigned int col = 0; col < n_cols; col++) + { + unsigned int min_x = original_bounds.getMinX() + small_width * col; + unsigned int max_x = std::min(min_x + small_width - 1, original_bounds.getMaxX()); + nav_core2::UIntBounds sub(min_x, min_y, max_x, max_y); + if (!sub.isEmpty()) + divided.push_back(sub); + } + } + return divided; +} +} // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/conversions.cpp b/src/Libraries/nav_2d_utils/src/conversions.cpp new file mode 100755 index 0000000..6e37674 --- /dev/null +++ b/src/Libraries/nav_2d_utils/src/conversions.cpp @@ -0,0 +1,339 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 + +namespace nav_2d_utils +{ + +geometry_msgs::Twist twist2Dto3D(const nav_2d_msgs::Twist2D& cmd_vel_2d) +{ + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = cmd_vel_2d.x; + cmd_vel.linear.y = cmd_vel_2d.y; + cmd_vel.angular.z = cmd_vel_2d.theta; + return cmd_vel; +} + + +nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist& cmd_vel) +{ + nav_2d_msgs::Twist2D cmd_vel_2d; + cmd_vel_2d.x = cmd_vel.linear.x; + cmd_vel_2d.y = cmd_vel.linear.y; + cmd_vel_2d.theta = cmd_vel.angular.z; + return cmd_vel_2d; +} + +nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point& point) +{ + nav_2d_msgs::Point2D output; + output.x = point.x; + output.y = point.y; + return output; +} + +nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32& point) +{ + nav_2d_msgs::Point2D output; + output.x = point.x; + output.y = point.y; + return output; +} + +geometry_msgs::Point pointToPoint3D(const nav_2d_msgs::Point2D& point) +{ + geometry_msgs::Point output; + output.x = point.x; + output.y = point.y; + return output; +} + +geometry_msgs::Point32 pointToPoint32(const nav_2d_msgs::Point2D& point) +{ + geometry_msgs::Point32 output; + output.x = point.x; + output.y = point.y; + return output; +} + +// nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped& pose) +// { +// nav_2d_msgs::Pose2DStamped pose2d; +// pose2d.header.stamp = pose.stamp_; +// pose2d.header.frame_id = pose.frame_id_; +// pose2d.pose.x = pose.getOrigin().getX(); +// pose2d.pose.y = pose.getOrigin().getY(); +// pose2d.pose.theta = tf::getYaw(pose.getRotation()); +// return pose2d; +// } + +nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped& pose) +{ + nav_2d_msgs::Pose2DStamped pose2d; + pose2d.header = pose.header; + pose2d.pose.x = pose.pose.position.x; + pose2d.pose.y = pose.pose.position.y; + // pose2d.pose.theta = tf::getYaw(pose.pose.orientation); + return pose2d; +} + +geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D& pose2d) +{ + geometry_msgs::Pose pose; + pose.position.x = pose2d.x; + pose.position.y = pose2d.y; + // pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); + return pose; +} + +geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped& pose2d) +{ + geometry_msgs::PoseStamped pose; + pose.header = pose2d.header; + pose.pose = pose2DToPose(pose2d.pose); + return pose; +} + +// geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d, +// const std::string& frame, const ros::Time& stamp) +// { +// geometry_msgs::PoseStamped pose; +// pose.header.frame_id = frame; +// pose.header.stamp = stamp; +// pose.pose.position.x = pose2d.x; +// pose.pose.position.y = pose2d.y; +// // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); +// return pose; +// } + +nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path& path) +{ + nav_2d_msgs::Path2D path2d; + path2d.header = path.header; + nav_2d_msgs::Pose2DStamped stamped_2d; + path2d.poses.resize(path.poses.size()); + for (unsigned int i = 0; i < path.poses.size(); i++) + { + stamped_2d = poseStampedToPose2D(path.poses[i]); + path2d.poses[i] = stamped_2d; + } + return path2d; +} + +nav_msgs::Path posesToPath(const std::vector& poses) +{ + nav_msgs::Path path; + if (poses.empty()) + return path; + + path.poses.resize(poses.size()); + path.header.frame_id = poses[0].header.frame_id; + path.header.stamp = poses[0].header.stamp; + for (unsigned int i = 0; i < poses.size(); i++) + { + path.poses[i] = poses[i]; + } + return path; +} + +nav_2d_msgs::Path2D posesToPath2D(const std::vector& poses) +{ + nav_2d_msgs::Path2D path; + if (poses.empty()) + return path; + + nav_2d_msgs::Pose2DStamped pose; + path.poses.resize(poses.size()); + path.header.frame_id = poses[0].header.frame_id; + path.header.stamp = poses[0].header.stamp; + for (unsigned int i = 0; i < poses.size(); i++) + { + pose = poseStampedToPose2D(poses[i]); + path.poses[i] = pose; + } + return path; +} + + +// nav_msgs::Path poses2DToPath(const std::vector& poses, +// const std::string& frame, const ros::Time& stamp) +// { +// nav_msgs::Path path; +// path.poses.resize(poses.size()); +// path.header.frame_id = frame; +// path.header.stamp = stamp; +// for (unsigned int i = 0; i < poses.size(); i++) +// { +// path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp); +// } +// return path; +// } + +nav_msgs::Path pathToPath(const nav_2d_msgs::Path2D& path2d) +{ + nav_msgs::Path path; + path.header = path2d.header; + path.poses.resize(path2d.poses.size()); + for (unsigned int i = 0; i < path.poses.size(); i++) + { + path.poses[i].header = path2d.header; + path.poses[i].pose = pose2DToPose(path2d.poses[i].pose); + } + return path; +} + +geometry_msgs::Polygon polygon2Dto3D(const nav_2d_msgs::Polygon2D& polygon_2d) +{ + geometry_msgs::Polygon polygon; + polygon.points.reserve(polygon_2d.points.size()); + for (const auto& pt : polygon_2d.points) + { + polygon.points.push_back(pointToPoint32(pt)); + } + return polygon; +} + +nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon& polygon_3d) +{ + nav_2d_msgs::Polygon2D polygon; + polygon.points.reserve(polygon_3d.points.size()); + for (const auto& pt : polygon_3d.points) + { + polygon.points.push_back(pointToPoint2D(pt)); + } + return polygon; +} + +geometry_msgs::PolygonStamped polygon2Dto3D(const nav_2d_msgs::Polygon2DStamped& polygon_2d) +{ + geometry_msgs::PolygonStamped polygon; + polygon.header = polygon_2d.header; + polygon.polygon = polygon2Dto3D(polygon_2d.polygon); + return polygon; +} + +nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped& polygon_3d) +{ + nav_2d_msgs::Polygon2DStamped polygon; + polygon.header = polygon_3d.header; + polygon.polygon = polygon3Dto2D(polygon_3d.polygon); + return polygon; +} + +nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo& info) +{ + nav_2d_msgs::NavGridInfo msg; + msg.width = info.width; + msg.height = info.height; + msg.resolution = info.resolution; + msg.frame_id = info.frame_id; + msg.origin_x = info.origin_x; + msg.origin_y = info.origin_y; + return msg; +} + +nav_grid::NavGridInfo fromMsg(const nav_2d_msgs::NavGridInfo& msg) +{ + nav_grid::NavGridInfo info; + info.width = msg.width; + info.height = msg.height; + info.resolution = msg.resolution; + info.frame_id = msg.frame_id; + info.origin_x = msg.origin_x; + info.origin_y = msg.origin_y; + return info; +} + +nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData& metadata, const std::string& frame) +{ + nav_grid::NavGridInfo info; + info.frame_id = frame; + info.resolution = metadata.resolution; + info.width = metadata.width; + info.height = metadata.height; + info.origin_x = metadata.origin.position.x; + info.origin_y = metadata.origin.position.y; + // if (std::abs(tf::getYaw(metadata.origin.orientation)) > 1e-5) + // { + // std::cout << "Conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring" << std::endl; + // } + return info; +} + +geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo& info) +{ + geometry_msgs::Pose origin; + origin.position.x = info.origin_x; + origin.position.y = info.origin_y; + origin.orientation.w = 1.0; + return origin; +} + +geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo& info) +{ + geometry_msgs::Pose2D origin; + origin.x = info.origin_x; + origin.y = info.origin_y; + return origin; +} + +nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo & info) +{ + nav_msgs::MapMetaData metadata; + metadata.resolution = info.resolution; + metadata.width = info.width; + metadata.height = info.height; + metadata.origin = getOrigin3D(info); + return metadata; +} + +nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds& bounds) +{ + nav_2d_msgs::UIntBounds msg; + msg.min_x = bounds.getMinX(); + msg.min_y = bounds.getMinY(); + msg.max_x = bounds.getMaxX(); + msg.max_y = bounds.getMaxY(); + return msg; +} + +nav_core2::UIntBounds fromMsg(const nav_2d_msgs::UIntBounds& msg) +{ + return nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y); +} + + +} // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/footprint.cpp b/src/Libraries/nav_2d_utils/src/footprint.cpp new file mode 100755 index 0000000..92ac1ea --- /dev/null +++ b/src/Libraries/nav_2d_utils/src/footprint.cpp @@ -0,0 +1,67 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 + +namespace nav_2d_utils +{ +nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write) +{ + std::string full_param_name; + nav_2d_msgs::Polygon2D footprint; + + // if (nh.searchParam("footprint", full_param_name)) + // { + // // footprint = polygonFromParams(nh, full_param_name, false); + // if (write) + // { + // nh.setParam("footprint", polygonToXMLRPC(footprint)); + // } + // } + // else if (nh.searchParam("robot_radius", full_param_name)) + // { + // double robot_radius = 0.0; + // nh.getParam(full_param_name, robot_radius); + // footprint = polygonFromRadius(robot_radius); + // if (write) + // { + // nh.setParam("robot_radius", robot_radius); + // } + // } + return footprint; +} + +} // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/nav_2d_utils/__init__.py b/src/Libraries/nav_2d_utils/src/nav_2d_utils/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py b/src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py new file mode 100755 index 0000000..0f05402 --- /dev/null +++ b/src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py @@ -0,0 +1,131 @@ +from geometry_msgs.msg import Pose, Pose2D, Quaternion, Point +from nav_2d_msgs.msg import Twist2D, Path2D, Pose2DStamped, Point2D +from nav_msgs.msg import Path +from geometry_msgs.msg import Twist, PoseStamped + +try: + from tf.transformations import euler_from_quaternion, quaternion_from_euler + + def get_yaw(orientation): + rpy = euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w)) + return rpy[2] + + def from_yaw(yaw): + q = Quaternion() + q.x, q.y, q.z, q.w = quaternion_from_euler(0, 0, yaw) + return q +except ImportError: + from math import sin, cos, atan2 + # From https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + + def from_yaw(yaw): + q = Quaternion() + q.z = sin(yaw * 0.5) + q.w = cos(yaw * 0.5) + return q + + def get_yaw(q): + siny_cosp = +2.0 * (q.w * q.z + q.x * q.y) + cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z) + return atan2(siny_cosp, cosy_cosp) + + +def twist2Dto3D(cmd_vel_2d): + cmd_vel = Twist() + cmd_vel.linear.x = cmd_vel_2d.x + cmd_vel.linear.y = cmd_vel_2d.y + cmd_vel.angular.z = cmd_vel_2d.theta + return cmd_vel + + +def twist3Dto2D(cmd_vel): + cmd_vel_2d = Twist2D() + cmd_vel_2d.x = cmd_vel.linear.x + cmd_vel_2d.y = cmd_vel.linear.y + cmd_vel_2d.theta = cmd_vel.angular.z + return cmd_vel_2d + + +def pointToPoint3D(point_2d): + point = Point() + point.x = point_2d.x + point.y = point_2d.y + return point + + +def pointToPoint2D(point): + point_2d = Point2D() + point_2d.x = point.x + point_2d.y = point.y + return point_2d + + +def poseToPose2D(pose): + pose2d = Pose2D() + pose2d.x = pose.position.x + pose2d.y = pose.position.y + pose2d.theta = get_yaw(pose.orientation) + return pose2d + + +def poseStampedToPose2DStamped(pose): + pose2d = Pose2DStamped() + pose2d.header = pose.header + pose2d.pose = poseToPose2D(pose.pose) + return pose2d + + +def poseToPose2DStamped(pose, frame, stamp): + pose2d = Pose2DStamped() + pose2d.header.frame_id = frame + pose2d.header.stamp = stamp + pose2d.pose = poseToPose2D(pose) + return pose2d + + +def pose2DToPose(pose2d): + pose = Pose() + pose.position.x = pose2d.x + pose.position.y = pose2d.y + pose.orientation = from_yaw(pose2d.theta) + return pose + + +def pose2DStampedToPoseStamped(pose2d): + pose = PoseStamped() + pose.header = pose2d.header + pose.pose = pose2DToPose(pose2d.pose) + return pose + + +def pose2DToPoseStamped(pose2d, frame, stamp): + pose = PoseStamped() + pose.header.frame_id = frame + pose.header.stamp = stamp + pose.pose.position.x = pose2d.x + pose.pose.position.y = pose2d.y + pose.pose.orientation = from_yaw(pose2d.theta) + return pose + + +def pathToPath2D(path): + path2d = Path2D() + if len(path.poses) == 0: + return path + path2d.header.frame_id = path.poses[0].header.frame_id + path2d.header.stamp = path.poses[0].header.stamp + for pose in path.poses: + stamped = poseStampedToPose2DStamped(pose) + path2d.poses.append(stamped.pose) + return path2d + + +def path2DToPath(path2d): + path = Path() + path.header = path2d.header + for pose2d in path2d.poses: + pose = PoseStamped() + pose.header = path2d.header + pose.pose = pose2DToPose(pose2d) + path.poses.append(pose) + return path diff --git a/src/Libraries/nav_2d_utils/src/path_ops.cpp b/src/Libraries/nav_2d_utils/src/path_ops.cpp new file mode 100755 index 0000000..a5a026a --- /dev/null +++ b/src/Libraries/nav_2d_utils/src/path_ops.cpp @@ -0,0 +1,193 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 + +namespace nav_2d_utils +{ + +double poseDistance(const geometry_msgs::Pose2D& pose0, const geometry_msgs::Pose2D& pose1) +{ + return hypot(pose0.x - pose1.x, pose0.y - pose1.y); +} + +double getPlanLength(const nav_2d_msgs::Path2D& plan, const unsigned int start_index) +{ + double length = 0.0; + for (unsigned int i = start_index + 1; i < plan.poses.size(); i++) + { + length += poseDistance(plan.poses[i - 1].pose, plan.poses[i].pose); + } + return length; +} + +double getPlanLength(const nav_2d_msgs::Path2D& plan, const geometry_msgs::Pose2D& query_pose) +{ + if (plan.poses.size() == 0) return 0.0; + + unsigned int closest_index = 0; + double closest_distance = poseDistance(plan.poses[0].pose, query_pose); + for (unsigned int i = 1; i < plan.poses.size(); i++) + { + double distance = poseDistance(plan.poses[i].pose, query_pose); + if (closest_distance > distance) + { + closest_index = i; + closest_distance = distance; + } + } + return getPlanLength(plan, closest_index); +} + +nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D& global_plan_in, double resolution) +{ + nav_2d_msgs::Path2D global_plan_out; + global_plan_out.header = global_plan_in.header; + if (global_plan_in.poses.size() == 0) + { + return global_plan_out; + } + + nav_2d_msgs::Pose2DStamped last_stp = global_plan_in.poses[0]; + global_plan_out.poses.push_back(last_stp); + + double sq_resolution = resolution * resolution; + geometry_msgs::Pose2D last = last_stp.pose; + for (unsigned int i = 1; i < global_plan_in.poses.size(); ++i) + { + geometry_msgs::Pose2D loop = global_plan_in.poses[i].pose; + double sq_dist = (loop.x - last.x) * (loop.x - last.x) + (loop.y - last.y) * (loop.y - last.y); + if (sq_dist > sq_resolution) + { + // add points in-between + double diff = sqrt(sq_dist) - resolution; + double steps_double = ceil(diff / resolution) + 1.0; + int steps = static_cast(steps_double); + + double delta_x = (loop.x - last.x) / steps_double; + double delta_y = (loop.y - last.y) / steps_double; + double delta_t = (loop.theta - last.theta) / steps_double; + + for (int j = 1; j < steps; ++j) + { + nav_2d_msgs::Pose2DStamped pose; + pose.header = last_stp.header; + pose.pose.x = last.x + j * delta_x; + pose.pose.y = last.y + j * delta_y; + pose.pose.theta = last.theta + j * delta_t; + global_plan_out.poses.push_back(pose); + } + } + global_plan_out.poses.push_back(global_plan_in.poses[i]); + last.x = loop.x; + last.y = loop.y; + } + return global_plan_out; +} + +using PoseList = std::vector; + +/** + * @brief Helper function for other version of compressPlan. + * + * Uses the Ramer Douglas Peucker algorithm. Ignores theta values + * + * @param input Full list of poses + * @param start_index Index of first pose (inclusive) + * @param end_index Index of last pose (inclusive) + * @param epsilon maximum allowable error. Increase for greater compression. + * @param list of poses possibly compressed for the poses[start_index, end_index] + */ +PoseList compressPlan(const PoseList& input, unsigned int start_index, unsigned int end_index, double epsilon) +{ + // Skip if only two points + if (end_index - start_index <= 1) + { + PoseList::const_iterator first = input.begin() + start_index; + PoseList::const_iterator last = input.begin() + end_index + 1; + return PoseList(first, last); + } + + // Find the point with the maximum distance to the line from start to end + const nav_2d_msgs::Pose2DStamped& start = input[start_index], + end = input[end_index]; + double max_distance = 0.0; + unsigned int max_index = 0; + for (unsigned int i = start_index + 1; i < end_index; i++) + { + const nav_2d_msgs::Pose2DStamped& pose = input[i]; + double d = distanceToLine(pose.pose.x, pose.pose.y, start.pose.x, start.pose.y, end.pose.x, end.pose.y); + if (d > max_distance) + { + max_index = i; + max_distance = d; + } + } + + // If max distance is less than epsilon, eliminate all the points between start and end + if (max_distance <= epsilon) + { + PoseList outer; + outer.push_back(start); + outer.push_back(end); + return outer; + } + + // If max distance is greater than epsilon, recursively simplify + PoseList first_part = compressPlan(input, start_index, max_index, epsilon); + PoseList second_part = compressPlan(input, max_index, end_index, epsilon); + first_part.insert(first_part.end(), second_part.begin() + 1, second_part.end()); + return first_part; +} + +nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D& input_path, double epsilon) +{ + nav_2d_msgs::Path2D results; + results.header = input_path.header; + results.poses = compressPlan(input_path.poses, 0, input_path.poses.size() - 1, epsilon); + return results; +} + +void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta) +{ + nav_2d_msgs::Pose2DStamped pose; + pose.pose.x = x; + pose.pose.y = y; + pose.pose.theta = theta; + path.poses.push_back(pose); +} +} // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/polygons.cpp b/src/Libraries/nav_2d_utils/src/polygons.cpp new file mode 100755 index 0000000..fc0beec --- /dev/null +++ b/src/Libraries/nav_2d_utils/src/polygons.cpp @@ -0,0 +1,502 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 +#include +#include + +namespace nav_2d_utils +{ + +using nav_2d_msgs::Point2D; +using nav_2d_msgs::Polygon2D; + +std::vector > parseVVD(const std::string& input) +{ + std::vector > result; + + std::stringstream input_ss(input); + int depth = 0; + std::vector current_vector; + while (input_ss.good()) + { + switch (input_ss.peek()) + { + case EOF: + break; + case '[': + depth++; + if (depth > 2) + { + throw PolygonParseException("Array depth greater than 2"); + } + input_ss.get(); + current_vector.clear(); + break; + case ']': + depth--; + if (depth < 0) + { + throw PolygonParseException("More close ] than open ["); + } + input_ss.get(); + if (depth == 1) + { + result.push_back(current_vector); + } + break; + case ',': + case ' ': + case '\t': + input_ss.get(); + break; + default: // All other characters should be part of the numbers. + if (depth != 2) + { + std::stringstream err_ss; + err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'."; + throw PolygonParseException(err_ss.str()); + } + double value; + if (input_ss >> value) + { + current_vector.push_back(value); + } + break; + } + } + + if (depth != 0) + { + throw PolygonParseException("Unterminated vector string."); + } + + return result; +} + +Polygon2D polygonFromRadius(const double radius, const unsigned int num_points) +{ + Polygon2D polygon; + Point2D pt; + for (unsigned int i = 0; i < num_points; ++i) + { + double angle = i * 2 * M_PI / num_points; + pt.x = cos(angle) * radius; + pt.y = sin(angle) * radius; + polygon.points.push_back(pt); + } + + return polygon; +} + +Polygon2D polygonFromString(const std::string& polygon_string) +{ + Polygon2D polygon; + // Will throw PolygonParseException if problematic + std::vector > vvd = parseVVD(polygon_string); + + // convert vvd into points. + if (vvd.size() < 3) + { + throw PolygonParseException("You must specify at least three points for the polygon."); + } + + polygon.points.resize(vvd.size()); + for (unsigned int i = 0; i < vvd.size(); i++) + { + if (vvd[ i ].size() != 2) + { + std::stringstream err_ss; + err_ss << "Points in the polygon specification must be pairs of numbers. Point index " << i << " had "; + err_ss << int(vvd[ i ].size()) << " numbers."; + throw PolygonParseException(err_ss.str()); + } + + polygon.points[i].x = vvd[i][0]; + polygon.points[i].y = vvd[i][1]; + } + + return polygon; +} + + +/** + * @brief Helper function. Convert value to double + */ +double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value) +{ + if (value.getType() == XmlRpc::XmlRpcValue::TypeInt) + { + return static_cast(static_cast(value)); + } + else if (value.getType() == XmlRpc::XmlRpcValue::TypeDouble) + { + return static_cast(value); + } + + std::stringstream err_ss; + err_ss << "Values in the polygon specification must be numbers. Found value: " << value.toXml(); + throw PolygonParseException(err_ss.str()); +} + +/** + * @brief Helper function. Convert value to double array + */ +std::vector getNumberVectorFromXMLRPC(XmlRpc::XmlRpcValue& value) +{ + if (value.getType() != XmlRpc::XmlRpcValue::TypeArray) + { + throw PolygonParseException("Subarray must have type list."); + } + std::vector array; + for (int i = 0; i < value.size(); i++) + { + array.push_back(getNumberFromXMLRPC(value[i])); + } + return array; +} + +Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc) +{ + if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString && + polygon_xmlrpc != "" && polygon_xmlrpc != "[]") + { + return polygonFromString(std::string(polygon_xmlrpc)); + } + + if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeStruct) + { + if (!polygon_xmlrpc.hasMember("x") || !polygon_xmlrpc.hasMember("y")) + { + throw PolygonParseException("Dict-like Polygon must specify members x and y."); + } + std::vector xs = getNumberVectorFromXMLRPC(polygon_xmlrpc["x"]); + std::vector ys = getNumberVectorFromXMLRPC(polygon_xmlrpc["y"]); + return polygonFromParallelArrays(xs, ys); + } + + // Make sure we have an array of at least 3 elements. + if (polygon_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray) + { + std::stringstream err_ss; + err_ss << "Polygon must be specified as a list of lists. Found object of type " << polygon_xmlrpc.getType() + << " instead."; + throw PolygonParseException(err_ss.str()); + } + else if (polygon_xmlrpc.size() < 3) + { + throw PolygonParseException("You must specify at least three points for the polygon."); + } + + Polygon2D polygon; + Point2D pt; + for (int i = 0; i < polygon_xmlrpc.size(); ++i) + { + // Make sure each element of the list is an array of size 2. (x and y coordinates) + XmlRpc::XmlRpcValue& point_xml = polygon_xmlrpc[i]; + if (point_xml.getType() != XmlRpc::XmlRpcValue::TypeArray) + { + std::stringstream err_ss; + err_ss << "Each point must be specified as a list. Found object of type " << point_xml.getType() << " instead."; + throw PolygonParseException(err_ss.str()); + } + else if (point_xml.size() != 2) + { + throw PolygonParseException("Each point must have two numbers (x and y)."); + } + + pt.x = getNumberFromXMLRPC(point_xml[0]); + pt.y = getNumberFromXMLRPC(point_xml[1]); + polygon.points.push_back(pt); + } + return polygon; +} + +// Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name, bool search) +// { +// std::string full_param_name; +// if (search) +// { +// nh.searchParam(parameter_name, full_param_name); +// } +// else +// { +// full_param_name = parameter_name; +// } + +// if (!nh.hasParam(full_param_name)) +// { +// std::stringstream err_ss; +// err_ss << "Parameter " << parameter_name << "(" + nh.resolveName(parameter_name) << ") not found."; +// throw PolygonParseException(err_ss.str()); +// } +// XmlRpc::XmlRpcValue polygon_xmlrpc; +// nh.getParam(full_param_name, polygon_xmlrpc); +// return polygonFromXMLRPC(polygon_xmlrpc); +// } + +/** + * @brief Helper method to convert a vector of doubles + */ +XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector& array) +{ + XmlRpc::XmlRpcValue xml; + xml.setSize(array.size()); + for (unsigned int i = 0; i < array.size(); ++i) + { + xml[i] = array[i]; + } + return xml; +} + +XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays) +{ + XmlRpc::XmlRpcValue xml; + if (array_of_arrays) + { + xml.setSize(polygon.points.size()); + for (unsigned int i = 0; i < polygon.points.size(); ++i) + { + xml[i].setSize(2); + const Point2D& p = polygon.points[i]; + xml[i][0] = p.x; + xml[i][1] = p.y; + } + } + else + { + std::vector xs, ys; + polygonToParallelArrays(polygon, xs, ys); + xml["x"] = vectorToXMLRPC(xs); + xml["y"] = vectorToXMLRPC(ys); + } + return xml; +} + +// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name, +// bool array_of_arrays) +// { +// nh.setParam(parameter_name, polygonToXMLRPC(polygon, array_of_arrays)); +// } + +nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, const std::vector& ys) +{ + if (xs.size() < 3) + { + throw PolygonParseException("You must specify at least three points for the polygon."); + } + else if (xs.size() != ys.size()) + { + throw PolygonParseException("Length of x array does not match length of y array."); + } + + Polygon2D polygon; + polygon.points.resize(xs.size()); + for (unsigned int i = 0; i < xs.size(); i++) + { + Point2D& pt = polygon.points[i]; + pt.x = xs[i]; + pt.y = ys[i]; + } + return polygon; +} + +void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector& xs, std::vector& ys) +{ + xs.clear(); + ys.clear(); + for (const Point2D& pt : polygon.points) + { + xs.push_back(pt.x); + ys.push_back(pt.y); + } +} + +bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D& polygon1) +{ + if (polygon0.points.size() != polygon1.points.size()) + { + return false; + } + for (unsigned int i=0; i < polygon0.points.size(); i++) + { + if (polygon0.points[i].x != polygon1.points[i].x ||polygon0.points[i].y != polygon1.points[i].y) + { + return false; + } + } + return true; +} + +nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon, + const geometry_msgs::Pose2D& pose) +{ + nav_2d_msgs::Polygon2D new_polygon; + new_polygon.points.resize(polygon.points.size()); + double cos_th = cos(pose.theta); + double sin_th = sin(pose.theta); + for (unsigned int i = 0; i < polygon.points.size(); ++i) + { + nav_2d_msgs::Point2D& new_pt = new_polygon.points[i]; + new_pt.x = pose.x + polygon.points[i].x * cos_th - polygon.points[i].y * sin_th; + new_pt.y = pose.y + polygon.points[i].x * sin_th + polygon.points[i].y * cos_th; + } + return new_polygon; +} + +bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const double y) +{ + // Determine if the given point is inside the polygon using the number of crossings method + // https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html + int n = polygon.points.size(); + int cross = 0; + // Loop from i = [0 ... n - 1] and j = [n - 1, 0 ... n - 2] + // Ensures first point connects to last point + for (int i = 0, j = n - 1; i < n; j = i++) + { + // Check if the line to x,y crosses this edge + if ( ((polygon.points[i].y > y) != (polygon.points[j].y > y)) + && (x < (polygon.points[j].x - polygon.points[i].x) * (y - polygon.points[i].y) / + (polygon.points[j].y - polygon.points[i].y) + polygon.points[i].x) ) + { + cross++; + } + } + // Return true if the number of crossings is odd + return cross % 2 > 0; +} + +void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist) +{ + min_dist = std::numeric_limits::max(); + max_dist = 0.0; + auto& points = polygon.points; + if (points.size() == 0) + { + return; + } + + for (unsigned int i = 0; i < points.size() - 1; ++i) + { + // check the distance from the robot center point to the first vertex + double vertex_dist = hypot(points[i].x, points[i].y); + double edge_dist = distanceToLine(0.0, 0.0, points[i].x, points[i].y, + points[i + 1].x, points[i + 1].y); + min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); + max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); + } + + // we also need to do the last vertex and the first vertex + double vertex_dist = hypot(points.back().x, points.back().y); + double edge_dist = distanceToLine(0.0, 0.0, points.back().x, points.back().y, + points.front().x, points.front().y); + min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); + max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); +} +} // namespace nav_2d_utils + +// Adapt Point2D for use with the triangulation library +namespace mapbox +{ +namespace util +{ +template <> +struct nth<0, nav_2d_msgs::Point2D> +{ + inline static double get(const nav_2d_msgs::Point2D& point) + { + return point.x; + }; +}; + +template <> +struct nth<1, nav_2d_msgs::Point2D> +{ + inline static double get(const nav_2d_msgs::Point2D& point) + { + return point.y; + }; +}; + +} // namespace util +} // namespace mapbox + + +namespace nav_2d_utils +{ +std::vector triangulate(const nav_2d_msgs::ComplexPolygon2D& polygon) +{ + // Compute the triangulation + std::vector> rings; + rings.reserve(1 + polygon.inner.size()); + rings.push_back(polygon.outer.points); + for (const nav_2d_msgs::Polygon2D& inner : polygon.inner) + { + rings.push_back(inner.points); + } + std::vector indices = mapbox::earcut(rings); + + // Create a sequential point index. The triangulation results are indices in this vector. + std::vector points; + for (const auto& ring : rings) + { + for (const nav_2d_msgs::Point2D& point : ring) + { + points.push_back(point); + } + } + + // Construct the output triangles + std::vector result; + result.reserve(indices.size()); + for (unsigned int index : indices) + { + result.push_back(points[index]); + } + return result; +} + +std::vector triangulate(const nav_2d_msgs::Polygon2D& polygon) +{ + nav_2d_msgs::ComplexPolygon2D complex; + complex.outer = polygon; + return triangulate(complex); +} + + +} // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/tf_help.cpp b/src/Libraries/nav_2d_utils/src/tf_help.cpp new file mode 100755 index 0000000..dc997b6 --- /dev/null +++ b/src/Libraries/nav_2d_utils/src/tf_help.cpp @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 2022, Metro Robots + * 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 nav_2d_utils +{ + bool transformPose(const TFListenerPtr tf, const std::string frame, + const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped &out_pose, + const bool extrapolation_fallback) + { + // if (in_pose.header.frame_id == frame) + // { + // out_pose = in_pose; + // return true; + // } + + // try + // { + // tf->transform(in_pose, out_pose, frame); + // return true; + // } + // catch (tf::ExtrapolationException &ex) + // { + // if (!extrapolation_fallback) + // throw; + // geometry_msgs::PoseStamped latest_in_pose; + // latest_in_pose.header.frame_id = in_pose.header.frame_id; + // latest_in_pose.pose = in_pose.pose; + // tf->transform(latest_in_pose, out_pose, frame); + // return true; + // } + // catch (tf::TransformException &ex) + // { + // ROS_ERROR("Exception in transformPose: %s", ex.what()); + // return false; + // } + return false; + } + + bool transformPose(const TFListenerPtr tf, const std::string frame, + const nav_2d_msgs::Pose2DStamped &in_pose, nav_2d_msgs::Pose2DStamped &out_pose, + const bool extrapolation_fallback) + { + geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose); + geometry_msgs::PoseStamped out_3d_pose; + + bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, extrapolation_fallback); + if (ret) + { + out_pose = poseStampedToPose2D(out_3d_pose); + } + return ret; + } + + geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose, + const std::string &frame_id) + { + nav_2d_msgs::Pose2DStamped local_pose; + nav_2d_utils::transformPose(tf, frame_id, pose, local_pose); + return local_pose.pose; + } + +} // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/test/bounds_test.cpp b/src/Libraries/nav_2d_utils/test/bounds_test.cpp new file mode 100755 index 0000000..58f840b --- /dev/null +++ b/src/Libraries/nav_2d_utils/test/bounds_test.cpp @@ -0,0 +1,199 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * 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 + +using nav_2d_utils::divideBounds; +using nav_core2::UIntBounds; + +/** + * @brief Count the values in a grid. + * @param[in] The grid + * @param[out] match Number of values == 1 + * @param[out] missed Number of values == 0 + * @param[out] multiple Number of other values + */ +void countValues(const nav_grid::VectorNavGrid& grid, + unsigned int& match, unsigned int& missed, unsigned int& multiple) +{ + match = 0; + missed = 0; + multiple = 0; + + nav_grid::NavGridInfo info = grid.getInfo(); + + // No iterator to avoid tricky depenencies + for (unsigned int x = 0; x < info.width; x++) + { + for (unsigned int y = 0; y < info.height; y++) + { + switch (grid(x, y)) + { + case 0: + missed++; + break; + case 1: + match++; + break; + default: + multiple++; + break; + } + } + } +} + +TEST(DivideBounds, zeroes) +{ + UIntBounds bounds(2, 2, 5, 5); + // Number of rows/cols has to be positive + EXPECT_THROW(divideBounds(bounds, 0, 2), std::invalid_argument); + EXPECT_THROW(divideBounds(bounds, 2, 0), std::invalid_argument); + EXPECT_THROW(divideBounds(bounds, 0, 0), std::invalid_argument); + EXPECT_NO_THROW(divideBounds(bounds, 2, 2)); + + bounds.reset(); + // check for errors with empty bounds + EXPECT_NO_THROW(divideBounds(bounds, 2, 2)); +} + +/** + * This test is for the divideBounds method and takes grids of various sizes + * (cycled through with the outer two loops) and tries to divide them into subgrids of + * various sizes (cycled through with the next two loops). The resulting vector of + * bounds should cover every cell in the original grid, so each of the divided bounds is + * iterated over, adding one to each grid cell. If everything works perfectly, each cell + * should be touched exactly once. + */ +TEST(DivideBounds, iterative_tests) +{ + nav_grid::VectorNavGrid full_grid; + nav_grid::NavGridInfo info; + + // count variables + unsigned int match, missed, multiple; + + for (info.width = 1; info.width < 15; info.width++) + { + for (info.height = 1; info.height < 15; info.height++) + { + full_grid.setInfo(info); + UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info); + for (unsigned int rows = 1; rows < 11u; rows++) + { + for (unsigned int cols = 1; cols < 11u; cols++) + { + full_grid.reset(); + std::vector divided = divideBounds(full_bounds, cols, rows); + ASSERT_LE(divided.size(), rows * cols) << info.width << "x" << info.height << " " << rows << "x" << cols; + for (const UIntBounds& sub : divided) + { + EXPECT_FALSE(sub.isEmpty()); + // Can't use nav_grid_iterator for circular dependencies + for (unsigned int x = sub.getMinX(); x <= sub.getMaxX(); x++) + { + for (unsigned int y = sub.getMinY(); y <= sub.getMaxY(); y++) + { + full_grid.setValue(x, y, full_grid(x, y) + 1); + } + } + } + + countValues(full_grid, match, missed, multiple); + ASSERT_EQ(match, info.width * info.height) << "Full grid: " << info.width << "x" << info.height + << " Requested divisions: " << rows << "x" << cols; + EXPECT_EQ(missed, 0u); + EXPECT_EQ(multiple, 0u); + } + } + } + } +} + +/** + * This test is for the divideBounds method and calls it recursively to + * ensure that the method works when the minimum values in the original bounds + * are not zero. + */ +TEST(DivideBounds, recursive_tests) +{ + nav_grid::VectorNavGrid full_grid; + nav_grid::NavGridInfo info; + info.width = 100; + info.height = 100; + full_grid.setInfo(info); + + UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info); + + std::vector level_one = divideBounds(full_bounds, 2, 2); + ASSERT_EQ(level_one.size(), 4u); + for (const UIntBounds& sub : level_one) + { + std::vector level_two = divideBounds(sub, 2, 2); + ASSERT_EQ(level_two.size(), 4u); + for (const UIntBounds& subsub : level_two) + { + EXPECT_GE(subsub.getMinX(), sub.getMinX()); + EXPECT_LE(subsub.getMaxX(), sub.getMaxX()); + EXPECT_GE(subsub.getMinY(), sub.getMinY()); + EXPECT_LE(subsub.getMaxY(), sub.getMaxY()); + // Can't use nav_grid_iterator for circular dependencies + for (unsigned int x = subsub.getMinX(); x <= subsub.getMaxX(); x++) + { + for (unsigned int y = subsub.getMinY(); y <= subsub.getMaxY(); y++) + { + full_grid.setValue(x, y, full_grid(x, y) + 1); + } + } + } + } + + // Count values + unsigned int match = 0, + missed = 0, + multiple = 0; + countValues(full_grid, match, missed, multiple); + ASSERT_EQ(match, info.width * info.height); + EXPECT_EQ(missed, 0u); + EXPECT_EQ(multiple, 0u); +} + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/Libraries/nav_2d_utils/test/compress_test.cpp b/src/Libraries/nav_2d_utils/test/compress_test.cpp new file mode 100755 index 0000000..41d5760 --- /dev/null +++ b/src/Libraries/nav_2d_utils/test/compress_test.cpp @@ -0,0 +1,97 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 + +using nav_2d_utils::compressPlan; +using nav_2d_utils::addPose; + +TEST(CompressTest, compress_test) +{ + nav_2d_msgs::Path2D path; + // Dataset borrowed from https://karthaus.nl/rdp/ + addPose(path, 24, 173); + addPose(path, 26, 170); + addPose(path, 24, 166); + addPose(path, 27, 162); + addPose(path, 37, 161); + addPose(path, 45, 157); + addPose(path, 48, 152); + addPose(path, 46, 143); + addPose(path, 40, 140); + addPose(path, 34, 137); + addPose(path, 26, 134); + addPose(path, 24, 130); + addPose(path, 24, 125); + addPose(path, 28, 121); + addPose(path, 36, 118); + addPose(path, 46, 117); + addPose(path, 63, 121); + addPose(path, 76, 125); + addPose(path, 82, 120); + addPose(path, 86, 111); + addPose(path, 88, 103); + addPose(path, 90, 91); + addPose(path, 95, 87); + addPose(path, 107, 89); + addPose(path, 107, 104); + addPose(path, 106, 117); + addPose(path, 109, 129); + addPose(path, 119, 131); + addPose(path, 131, 131); + addPose(path, 139, 134); + addPose(path, 138, 143); + addPose(path, 131, 152); + addPose(path, 119, 154); + addPose(path, 111, 149); + addPose(path, 105, 143); + addPose(path, 91, 139); + addPose(path, 80, 142); + addPose(path, 81, 152); + addPose(path, 76, 163); + addPose(path, 67, 161); + addPose(path, 59, 149); + addPose(path, 63, 138); + + EXPECT_EQ(41U, compressPlan(path, 0.1).poses.size()); + EXPECT_EQ(34U, compressPlan(path, 1.3).poses.size()); + EXPECT_EQ(12U, compressPlan(path, 9.5).poses.size()); + EXPECT_EQ(8U, compressPlan(path, 19.9).poses.size()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/Libraries/nav_2d_utils/test/param_tests.cpp b/src/Libraries/nav_2d_utils/test/param_tests.cpp new file mode 100755 index 0000000..5ac252e --- /dev/null +++ b/src/Libraries/nav_2d_utils/test/param_tests.cpp @@ -0,0 +1,146 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* 2018, Locus Robotics +* 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 Willow Garage, Inc. 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. +* +* Author: Dave Hershberger +* David V. Lu!! (nav_2d_utils version) +*********************************************************************/ +#include +#include +#include + +using nav_2d_utils::polygonFromParams; +using nav_2d_msgs::Polygon2D; + +TEST(Polygon2D, unpadded_footprint_from_string_param) +{ + YAML::Node nh("~unpadded"); + Polygon2D footprint = polygonFromParams(nh, "footprint"); + ASSERT_EQ(3U, footprint.points.size()); + + EXPECT_EQ(1.0f, footprint.points[ 0 ].x); + EXPECT_EQ(1.0f, footprint.points[ 0 ].y); + + EXPECT_EQ(-1.0f, footprint.points[ 1 ].x); + EXPECT_EQ(1.0f, footprint.points[ 1 ].y); + + EXPECT_EQ(-1.0f, footprint.points[ 2 ].x); + EXPECT_EQ(-1.0f, footprint.points[ 2 ].y); +} + +TEST(Polygon2D, check_search_capabilities) +{ + YAML::Node nh("~unpadded/unneccessarily/long_namespace"); + Polygon2D footprint = polygonFromParams(nh, "footprint"); + ASSERT_EQ(3U, footprint.points.size()); + EXPECT_THROW(polygonFromParams(nh, "footprint", false), nav_2d_utils::PolygonParseException); +} + +TEST(Polygon2D, footprint_from_xmlrpc_param) +{ + YAML::Node nh("~xmlrpc"); + Polygon2D footprint = polygonFromParams(nh, "footprint"); + ASSERT_EQ(4U, footprint.points.size()); + + EXPECT_FLOAT_EQ(0.1f, footprint.points[ 0 ].x); + EXPECT_FLOAT_EQ(0.1f, footprint.points[ 0 ].y); + + EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 1 ].x); + EXPECT_FLOAT_EQ(0.1f, footprint.points[ 1 ].y); + + EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 2 ].x); + EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 2 ].y); + + EXPECT_FLOAT_EQ(0.1f, footprint.points[ 3 ].x); + EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 3 ].y); + + Polygon2D footprint2 = polygonFromParams(nh, "footprint2"); + ASSERT_TRUE(nav_2d_utils::equals(footprint, footprint2)); +} + +TEST(Polygon2D, footprint_from_same_level_param) +{ + YAML::Node nh("~same_level"); + Polygon2D footprint = polygonFromParams(nh, "footprint"); + ASSERT_EQ(3U, footprint.points.size()); + + EXPECT_EQ(1.0f, footprint.points[ 0 ].x); + EXPECT_EQ(2.0f, footprint.points[ 0 ].y); + + EXPECT_EQ(3.0f, footprint.points[ 1 ].x); + EXPECT_EQ(4.0f, footprint.points[ 1 ].y); + + EXPECT_EQ(5.0f, footprint.points[ 2 ].x); + EXPECT_EQ(6.0f, footprint.points[ 2 ].y); +} + +TEST(Polygon2D, footprint_from_xmlrpc_param_failure) +{ + YAML::Node nh("~xmlrpc_fail"); + EXPECT_THROW(polygonFromParams(nh, "footprint"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint2"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint3"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint4"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint5"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint6"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint7"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint8"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint9"), nav_2d_utils::PolygonParseException); +} + +TEST(Polygon2D, footprint_empty) +{ + YAML::Node nh("~empty"); + EXPECT_THROW(polygonFromParams(nh, "footprint"), nav_2d_utils::PolygonParseException); +} + +TEST(Polygon2D, test_write) +{ + YAML::Node nh("~unpadded"); + Polygon2D footprint = polygonFromParams(nh, "footprint"); + nh.setParam("another_footprint", nav_2d_utils::polygonToXMLRPC(footprint)); + Polygon2D another_footprint = polygonFromParams(nh, "another_footprint"); + EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint)); + + nh.setParam("third_footprint", nav_2d_utils::polygonToXMLRPC(footprint, false)); + another_footprint = polygonFromParams(nh, "third_footprint"); + EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint)); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "param_tests"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/Libraries/nav_2d_utils/test/param_tests.launch b/src/Libraries/nav_2d_utils/test/param_tests.launch new file mode 100755 index 0000000..ead5813 --- /dev/null +++ b/src/Libraries/nav_2d_utils/test/param_tests.launch @@ -0,0 +1,34 @@ + + + + + + footprint: [[0.1, 0.1], [-0.1, 0.1], [-0.1, -0.1], [0.1, -0.1]] + footprint2: + x: [0.1, -0.1, -0.1, 0.1] + y: [0.1, 0.1, -0.1, -0.1] + + + + footprint: [[0.1, 0.1], [-0.1, 0.1, 77.0], [-0.1, -0.1], [0.1, -0.1]] + footprint2: 1.0 + footprint3: false + footprint4: [[0.1, 0.1], [0.1, 0.1]] + footprint5: ['x', 'y', 'z'] + footprint6: [['x', 'y'], ['a', 'b'], ['c'], ['d']] + footprint7: + x: [0.1, -0.1, -0.1, 0.1] + footprint8: + x: 0 + y: 0 + footprint9: + x: ['a', 'b', 'c'] + y: [d, e, f] + + + + + + + + diff --git a/src/Libraries/nav_2d_utils/test/polygon_tests.cpp b/src/Libraries/nav_2d_utils/test/polygon_tests.cpp new file mode 100755 index 0000000..9bbc220 --- /dev/null +++ b/src/Libraries/nav_2d_utils/test/polygon_tests.cpp @@ -0,0 +1,185 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 + +using nav_2d_utils::parseVVD; +using nav_2d_msgs::Polygon2D; +using nav_2d_utils::polygonFromString; +using nav_2d_utils::polygonFromParallelArrays; + +TEST(array_parser, basic_operation) +{ + std::vector > vvd; + vvd = parseVVD("[[1, 2.2], [.3, -4e4]]"); + EXPECT_DOUBLE_EQ(2U, vvd.size()); + EXPECT_DOUBLE_EQ(2U, vvd[0].size()); + EXPECT_DOUBLE_EQ(2U, vvd[1].size()); + EXPECT_DOUBLE_EQ(1.0, vvd[0][0]); + EXPECT_DOUBLE_EQ(2.2, vvd[0][1]); + EXPECT_DOUBLE_EQ(0.3, vvd[1][0]); + EXPECT_DOUBLE_EQ(-40000.0, vvd[1][1]); +} + +TEST(array_parser, missing_open) +{ + EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]]"), nav_2d_utils::PolygonParseException); +} + +TEST(array_parser, missing_close) +{ + EXPECT_THROW(parseVVD("[[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException); +} + +TEST(array_parser, wrong_depth) +{ + EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException); +} + +TEST(Polygon2D, radius_param) +{ + Polygon2D footprint = nav_2d_utils::polygonFromRadius(10.0); + // Circular robot has 16-point footprint auto-generated. + ASSERT_EQ(16U, footprint.points.size()); + + // Check the first point + EXPECT_EQ(10.0, footprint.points[0].x); + EXPECT_EQ(0.0, footprint.points[0].y); + + // Check the 4th point, which should be 90 degrees around the circle from the first. + EXPECT_NEAR(0.0, footprint.points[4].x, 0.0001); + EXPECT_NEAR(10.0, footprint.points[4].y, 0.0001); +} + +TEST(Polygon2D, string_param) +{ + Polygon2D footprint = polygonFromString("[[1, 1], [-1, 1], [-1, -1]]"); + ASSERT_EQ(3U, footprint.points.size()); + + EXPECT_EQ(1.0, footprint.points[ 0 ].x); + EXPECT_EQ(1.0, footprint.points[ 0 ].y); + + EXPECT_EQ(-1.0, footprint.points[ 1 ].x); + EXPECT_EQ(1.0, footprint.points[ 1 ].y); + + EXPECT_EQ(-1.0, footprint.points[ 2 ].x); + EXPECT_EQ(-1.0, footprint.points[ 2 ].y); +} + +TEST(Polygon2D, broken_string_param) +{ + // Not enough points + EXPECT_THROW(polygonFromString("[[1, 1], [-1, 1]]"), nav_2d_utils::PolygonParseException); + + // Too many numbers in point + EXPECT_THROW(polygonFromString("[[1, 1, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException); + + // Unexpected character + EXPECT_THROW(polygonFromString("[[x, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException); + + // Empty String + EXPECT_THROW(polygonFromString(""), nav_2d_utils::PolygonParseException); + + // Empty List + EXPECT_THROW(polygonFromString("[]"), nav_2d_utils::PolygonParseException); + + // Empty Point + EXPECT_THROW(polygonFromString("[[]]"), nav_2d_utils::PolygonParseException); +} + +TEST(Polygon2D, arrays) +{ + std::vector xs = {1, -1, -1}; + std::vector ys = {1, 1, -1}; + Polygon2D footprint = polygonFromParallelArrays(xs, ys); + ASSERT_EQ(3U, footprint.points.size()); + + EXPECT_EQ(1.0, footprint.points[ 0 ].x); + EXPECT_EQ(1.0, footprint.points[ 0 ].y); + + EXPECT_EQ(-1.0, footprint.points[ 1 ].x); + EXPECT_EQ(1.0, footprint.points[ 1 ].y); + + EXPECT_EQ(-1.0, footprint.points[ 2 ].x); + EXPECT_EQ(-1.0, footprint.points[ 2 ].y); +} + +TEST(Polygon2D, broken_arrays) +{ + std::vector shorty = {1, -1}; + std::vector three = {1, 1, -1}; + std::vector four = {1, 1, -1, -1}; + EXPECT_THROW(polygonFromParallelArrays(shorty, shorty), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParallelArrays(three, four), nav_2d_utils::PolygonParseException); +} + +TEST(Polygon2D, test_move) +{ + Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"); + geometry_msgs::Pose2D pose; + Polygon2D square2 = nav_2d_utils::movePolygonToPose(square, pose); + EXPECT_TRUE(nav_2d_utils::equals(square, square2)); + pose.x = 15; + pose.y = -10; + pose.theta = M_PI / 4; + Polygon2D diamond = nav_2d_utils::movePolygonToPose(square, pose); + ASSERT_EQ(4U, diamond.points.size()); + double side = 1.0 / sqrt(2); + + EXPECT_DOUBLE_EQ(pose.x, diamond.points[ 0 ].x); + EXPECT_DOUBLE_EQ(pose.y + side, diamond.points[ 0 ].y); + EXPECT_DOUBLE_EQ(pose.x + side, diamond.points[ 1 ].x); + EXPECT_DOUBLE_EQ(pose.y, diamond.points[ 1 ].y); + EXPECT_DOUBLE_EQ(pose.x, diamond.points[ 2 ].x); + EXPECT_DOUBLE_EQ(pose.y - side, diamond.points[ 2 ].y); + EXPECT_DOUBLE_EQ(pose.x - side, diamond.points[ 3 ].x); + EXPECT_DOUBLE_EQ(pose.y, diamond.points[ 3 ].y); +} + +TEST(Polygon2D, inside) +{ + Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"); + EXPECT_TRUE(nav_2d_utils::isInside(square, 0.00, 0.00)); + EXPECT_TRUE(nav_2d_utils::isInside(square, 0.45, 0.45)); + EXPECT_FALSE(nav_2d_utils::isInside(square, 0.50, 0.50)); + EXPECT_FALSE(nav_2d_utils::isInside(square, 0.00, 0.50)); + EXPECT_FALSE(nav_2d_utils::isInside(square, 0.55, 0.55)); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/Libraries/nav_2d_utils/test/resolution_test.cpp b/src/Libraries/nav_2d_utils/test/resolution_test.cpp new file mode 100755 index 0000000..1a676b5 --- /dev/null +++ b/src/Libraries/nav_2d_utils/test/resolution_test.cpp @@ -0,0 +1,78 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 + +using nav_2d_utils::adjustPlanResolution; +using nav_2d_utils::addPose; + +TEST(ResolutionTest, simple_example) +{ + nav_2d_msgs::Path2D path; + // Space between points is one meter + addPose(path, 0.0, 0.0); + addPose(path, 0.0, 1.0); + + // resolution>=1, path won't change + EXPECT_EQ(2U, adjustPlanResolution(path, 2.0).poses.size()); + EXPECT_EQ(2U, adjustPlanResolution(path, 1.0).poses.size()); + + // 0.5 <= resolution < 1.0, one point should be added in the middle + EXPECT_EQ(3U, adjustPlanResolution(path, 0.8).poses.size()); + EXPECT_EQ(3U, adjustPlanResolution(path, 0.5).poses.size()); + + // 0.333 <= resolution < 0.5, two points need to be added + EXPECT_EQ(4U, adjustPlanResolution(path, 0.34).poses.size()); + + // 0.25 <= resolution < 0.333, three points need to be added + EXPECT_EQ(5U, adjustPlanResolution(path, 0.32).poses.size()); +} + +TEST(ResolutionTest, real_example) +{ + // This test is based on a real-world example + nav_2d_msgs::Path2D path; + addPose(path, 17.779193, -0.972024); + addPose(path, 17.799171, -0.950775); + addPose(path, 17.851942, -0.903709); + EXPECT_EQ(3U, adjustPlanResolution(path, 0.2).poses.size()); + EXPECT_EQ(4U, adjustPlanResolution(path, 0.05).poses.size()); +} + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/Libraries/node_handle/include/robot/console.h b/src/Libraries/node_handle/include/robot/console.h new file mode 100644 index 0000000..41102f3 --- /dev/null +++ b/src/Libraries/node_handle/include/robot/console.h @@ -0,0 +1,89 @@ +#ifndef ROBOT_CONSOLE_H +#define ROBOT_CONSOLE_H + +#include +#include + +namespace robot { + +// ANSI Color Codes +namespace color { + // Reset + static const char* RESET = "\033[0m"; + + // Text colors + static const char* BLACK = "\033[30m"; + static const char* RED = "\033[31m"; + static const char* GREEN = "\033[32m"; + static const char* YELLOW = "\033[33m"; + static const char* BLUE = "\033[34m"; + static const char* MAGENTA = "\033[35m"; + static const char* CYAN = "\033[36m"; + static const char* WHITE = "\033[37m"; + + // Bright text colors + static const char* BRIGHT_BLACK = "\033[90m"; + static const char* BRIGHT_RED = "\033[91m"; + static const char* BRIGHT_GREEN = "\033[92m"; + static const char* BRIGHT_YELLOW = "\033[93m"; + static const char* BRIGHT_BLUE = "\033[94m"; + static const char* BRIGHT_MAGENTA = "\033[95m"; + static const char* BRIGHT_CYAN = "\033[96m"; + static const char* BRIGHT_WHITE = "\033[97m"; + + // Background colors + static const char* BG_BLACK = "\033[40m"; + static const char* BG_RED = "\033[41m"; + static const char* BG_GREEN = "\033[42m"; + static const char* BG_YELLOW = "\033[43m"; + static const char* BG_BLUE = "\033[44m"; + static const char* BG_MAGENTA = "\033[45m"; + static const char* BG_CYAN = "\033[46m"; + static const char* BG_WHITE = "\033[47m"; + + // Text styles + static const char* BOLD = "\033[1m"; + static const char* DIM = "\033[2m"; + static const char* ITALIC = "\033[3m"; + static const char* UNDERLINE = "\033[4m"; + static const char* BLINK = "\033[5m"; + static const char* REVERSE = "\033[7m"; +} + +// Check if terminal supports colors +bool is_color_supported(); + +// Enable/disable color output (useful for non-terminal outputs) +void set_color_enabled(bool enabled); +bool is_color_enabled(); + +// Colored printf functions +void printf_red(const char* format, ...); +void printf_green(const char* format, ...); +void printf_yellow(const char* format, ...); +void printf_blue(const char* format, ...); +void printf_cyan(const char* format, ...); +void printf_magenta(const char* format, ...); +void printf_white(const char* format, ...); + +// Colored printf with custom color +void printf_color(const char* color_code, const char* format, ...); + +// Log level functions +void log_info(const char* format, ...); +void log_success(const char* format, ...); +void log_warning(const char* format, ...); +void log_error(const char* format, ...); +void log_debug(const char* format, ...); + +// Print with file and line info (colored) +void log_info_at(const char* file, int line, const char* format, ...); +void log_success_at(const char* file, int line, const char* format, ...); +void log_warning_at(const char* file, int line, const char* format, ...); +void log_error_at(const char* file, int line, const char* format, ...); +void log_debug_at(const char* file, int line, const char* format, ...); + +} // namespace robot + +#endif // ROBOT_CONSOLE_H + diff --git a/src/Libraries/node_handle/src/console.cpp b/src/Libraries/node_handle/src/console.cpp new file mode 100644 index 0000000..299f88d --- /dev/null +++ b/src/Libraries/node_handle/src/console.cpp @@ -0,0 +1,207 @@ +#include "robot/console.h" +#include +#include + +namespace robot { + +// Global flag to enable/disable colors +static bool color_enabled = true; + +bool is_color_supported() { + // Check if NO_COLOR environment variable is set + const char* no_color = std::getenv("NO_COLOR"); + if (no_color != nullptr && strlen(no_color) > 0) { + return false; + } + + // Check if TERM environment variable suggests color support + const char* term = std::getenv("TERM"); + if (term != nullptr) { + // Common terminals that support colors + if (strstr(term, "xterm") != nullptr || + strstr(term, "color") != nullptr || + strstr(term, "256") != nullptr || + strstr(term, "screen") != nullptr || + strstr(term, "tmux") != nullptr) { + return true; + } + } + + // Default to true for most modern terminals + return true; +} + +void set_color_enabled(bool enabled) { + color_enabled = enabled && is_color_supported(); +} + +bool is_color_enabled() { + return color_enabled && is_color_supported(); +} + +// Helper function that accepts va_list +static void vprintf_color(const char* color_code, const char* format, va_list args) { + if (is_color_enabled()) { + std::printf("%s", color_code); + } + + std::vprintf(format, args); + + if (is_color_enabled()) { + std::printf("%s", color::RESET); + } +} + +void printf_color(const char* color_code, const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color_code, format, args); + va_end(args); +} + +void printf_red(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::RED, format, args); + va_end(args); +} + +void printf_green(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::GREEN, format, args); + va_end(args); +} + +void printf_yellow(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::YELLOW, format, args); + va_end(args); +} + +void printf_blue(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::BLUE, format, args); + va_end(args); +} + +void printf_cyan(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::CYAN, format, args); + va_end(args); +} + +void printf_magenta(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::MAGENTA, format, args); + va_end(args); +} + +void printf_white(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::WHITE, format, args); + va_end(args); +} + +void log_info(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::BLUE, format, args); + va_end(args); +} + +void log_success(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::GREEN, format, args); + va_end(args); +} + +void log_warning(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::YELLOW, format, args); + va_end(args); +} + +void log_error(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::RED, format, args); + va_end(args); +} + +void log_debug(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::CYAN, format, args); + va_end(args); +} + +void log_info_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[INFO]%s [%s:%d] ", color::BLUE, color::RESET, file, line); + } else { + std::printf("[INFO] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +void log_success_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[SUCCESS]%s [%s:%d] ", color::GREEN, color::RESET, file, line); + } else { + std::printf("[SUCCESS] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +void log_warning_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[WARNING]%s [%s:%d] ", color::YELLOW, color::RESET, file, line); + } else { + std::printf("[WARNING] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +void log_error_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[ERROR]%s [%s:%d] ", color::RED, color::RESET, file, line); + } else { + std::printf("[ERROR] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +void log_debug_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[DEBUG]%s [%s:%d] ", color::CYAN, color::RESET, file, line); + } else { + std::printf("[DEBUG] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +} // namespace robot + diff --git a/src/Libraries/robot_cpp/CMakeLists.txt b/src/Libraries/robot_cpp/CMakeLists.txt new file mode 100644 index 0000000..8789faa --- /dev/null +++ b/src/Libraries/robot_cpp/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.10) +project(robot_cpp) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -fPIC) +endif() + +add_library(${PROJECT_NAME}_node_handle SHARED + src/node_handle.cpp +) + +target_include_directories(${PROJECT_NAME}_node_handle + PUBLIC + $ + $ +) + +target_link_libraries(${PROJECT_NAME}_node_handle + PUBLIC + yaml-cpp + xmlrpcpp +) + +install(TARGETS ${PROJECT_NAME}_node_handle + EXPORT ${PROJECT_NAME}_node_handle-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +install(EXPORT ${PROJECT_NAME}_node_handle-targets + NAMESPACE robot:: + DESTINATION lib/cmake/${PROJECT_NAME}_node_handle) + +# Create alias for easier usage +add_library(robot::node_handle ALIAS ${PROJECT_NAME}_node_handle) + + +add_library(${PROJECT_NAME}_console SHARED + src/console.cpp +) + +target_include_directories(${PROJECT_NAME}_console + PUBLIC + $ + $ +) + +# Console library only needs standard C++ library, no external dependencies needed + +install(TARGETS ${PROJECT_NAME}_console + EXPORT ${PROJECT_NAME}_console-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(EXPORT ${PROJECT_NAME}_console-targets + NAMESPACE robot:: + DESTINATION lib/cmake/${PROJECT_NAME}_console) + +# Create alias for easier usage +add_library(robot::console ALIAS ${PROJECT_NAME}_console) diff --git a/src/Libraries/robot_cpp/include/robot/console.h b/src/Libraries/robot_cpp/include/robot/console.h new file mode 100644 index 0000000..41102f3 --- /dev/null +++ b/src/Libraries/robot_cpp/include/robot/console.h @@ -0,0 +1,89 @@ +#ifndef ROBOT_CONSOLE_H +#define ROBOT_CONSOLE_H + +#include +#include + +namespace robot { + +// ANSI Color Codes +namespace color { + // Reset + static const char* RESET = "\033[0m"; + + // Text colors + static const char* BLACK = "\033[30m"; + static const char* RED = "\033[31m"; + static const char* GREEN = "\033[32m"; + static const char* YELLOW = "\033[33m"; + static const char* BLUE = "\033[34m"; + static const char* MAGENTA = "\033[35m"; + static const char* CYAN = "\033[36m"; + static const char* WHITE = "\033[37m"; + + // Bright text colors + static const char* BRIGHT_BLACK = "\033[90m"; + static const char* BRIGHT_RED = "\033[91m"; + static const char* BRIGHT_GREEN = "\033[92m"; + static const char* BRIGHT_YELLOW = "\033[93m"; + static const char* BRIGHT_BLUE = "\033[94m"; + static const char* BRIGHT_MAGENTA = "\033[95m"; + static const char* BRIGHT_CYAN = "\033[96m"; + static const char* BRIGHT_WHITE = "\033[97m"; + + // Background colors + static const char* BG_BLACK = "\033[40m"; + static const char* BG_RED = "\033[41m"; + static const char* BG_GREEN = "\033[42m"; + static const char* BG_YELLOW = "\033[43m"; + static const char* BG_BLUE = "\033[44m"; + static const char* BG_MAGENTA = "\033[45m"; + static const char* BG_CYAN = "\033[46m"; + static const char* BG_WHITE = "\033[47m"; + + // Text styles + static const char* BOLD = "\033[1m"; + static const char* DIM = "\033[2m"; + static const char* ITALIC = "\033[3m"; + static const char* UNDERLINE = "\033[4m"; + static const char* BLINK = "\033[5m"; + static const char* REVERSE = "\033[7m"; +} + +// Check if terminal supports colors +bool is_color_supported(); + +// Enable/disable color output (useful for non-terminal outputs) +void set_color_enabled(bool enabled); +bool is_color_enabled(); + +// Colored printf functions +void printf_red(const char* format, ...); +void printf_green(const char* format, ...); +void printf_yellow(const char* format, ...); +void printf_blue(const char* format, ...); +void printf_cyan(const char* format, ...); +void printf_magenta(const char* format, ...); +void printf_white(const char* format, ...); + +// Colored printf with custom color +void printf_color(const char* color_code, const char* format, ...); + +// Log level functions +void log_info(const char* format, ...); +void log_success(const char* format, ...); +void log_warning(const char* format, ...); +void log_error(const char* format, ...); +void log_debug(const char* format, ...); + +// Print with file and line info (colored) +void log_info_at(const char* file, int line, const char* format, ...); +void log_success_at(const char* file, int line, const char* format, ...); +void log_warning_at(const char* file, int line, const char* format, ...); +void log_error_at(const char* file, int line, const char* format, ...); +void log_debug_at(const char* file, int line, const char* format, ...); + +} // namespace robot + +#endif // ROBOT_CONSOLE_H + diff --git a/src/Libraries/robot_cpp/include/robot/node_handle.h b/src/Libraries/robot_cpp/include/robot/node_handle.h new file mode 100644 index 0000000..313912c --- /dev/null +++ b/src/Libraries/robot_cpp/include/robot/node_handle.h @@ -0,0 +1,83 @@ +#ifndef ROBOT_NODE_H_INCLUDED_H +#define ROBOT_NODE_H_INCLUDED_H + +#include +#include +#include +#include +#include + +namespace robot +{ + class NodeHandle + { + public: + using Ptr = std::shared_ptr; + + NodeHandle(); + + NodeHandle(const std::string &name); + + ~NodeHandle(); + + bool getParam (const std::string &key, bool &b, bool default_value = false) const; + + bool getParam (const std::string &key, double &d, double default_value = 0.0) const; + + bool getParam (const std::string &key, float &f, float default_value = 0.0) const; + + bool getParam (const std::string &key, int &i, int default_value = 0) const; + + bool getParam (const std::string &key, std::map< std::string, bool > &map, std::map< std::string, bool > default_value = std::map< std::string, bool >()) const; + + bool getParam (const std::string &key, std::map< std::string, double > &map, std::map< std::string, double > default_value = std::map< std::string, double >()) const; + + bool getParam (const std::string &key, std::map< std::string, float > &map, std::map< std::string, float > default_value = std::map< std::string, float >()) const; + + bool getParam (const std::string &key, std::map< std::string, int > &map, std::map< std::string, int > default_value = std::map< std::string, int >()) const; + + bool getParam (const std::string &key, std::map< std::string, std::string > &map, std::map< std::string, std::string > default_value = std::map< std::string, std::string >()) const; + + bool getParam (const std::string &key, std::string &s, std::string default_value = "") const; + + bool getParam (const std::string &key, std::vector< bool > &vec, std::vector< bool > default_value = std::vector< bool >()) const; + + bool getParam (const std::string &key, std::vector< double > &vec, std::vector< double > default_value = std::vector< double >()) const; + + bool getParam (const std::string &key, std::vector< float > &vec, std::vector< float > default_value = std::vector< float >()) const; + + bool getParam (const std::string &key, std::vector< int > &vec, std::vector< int > default_value = std::vector< int >()) const; + + bool getParam (const std::string &key, std::vector< std::string > &vec, std::vector< std::string > default_value = std::vector< std::string >()) const; + + bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const; + + // Helper method to set parameters from YAML::Node + void setParam(const std::string &key, const YAML::Node &value); + + // Overloads for basic types + void setParam(const std::string &key, bool value); + void setParam(const std::string &key, int value); + void setParam(const std::string &key, double value); + void setParam(const std::string &key, float value); + void setParam(const std::string &key, const std::string &value); + + // Helper method to get nested parameter by key path (e.g., "parent/child") + YAML::Node getParamValue(const std::string &key) const; + + // Load YAML file + bool loadYamlFile(const std::string &filepath); + + // Get namespace + std::string getNamespace() const; + + private: + std::string namespace_; + YAML::Node node_handle_; + + // Helper method to split key path and get nested value + YAML::Node getNestedValue(const std::string &key) const; + }; +} // namespace robot + +#endif // ROBOT_NODE_H_INCLUDED_H \ No newline at end of file diff --git a/src/Libraries/robot_cpp/src/console.cpp b/src/Libraries/robot_cpp/src/console.cpp new file mode 100644 index 0000000..299f88d --- /dev/null +++ b/src/Libraries/robot_cpp/src/console.cpp @@ -0,0 +1,207 @@ +#include "robot/console.h" +#include +#include + +namespace robot { + +// Global flag to enable/disable colors +static bool color_enabled = true; + +bool is_color_supported() { + // Check if NO_COLOR environment variable is set + const char* no_color = std::getenv("NO_COLOR"); + if (no_color != nullptr && strlen(no_color) > 0) { + return false; + } + + // Check if TERM environment variable suggests color support + const char* term = std::getenv("TERM"); + if (term != nullptr) { + // Common terminals that support colors + if (strstr(term, "xterm") != nullptr || + strstr(term, "color") != nullptr || + strstr(term, "256") != nullptr || + strstr(term, "screen") != nullptr || + strstr(term, "tmux") != nullptr) { + return true; + } + } + + // Default to true for most modern terminals + return true; +} + +void set_color_enabled(bool enabled) { + color_enabled = enabled && is_color_supported(); +} + +bool is_color_enabled() { + return color_enabled && is_color_supported(); +} + +// Helper function that accepts va_list +static void vprintf_color(const char* color_code, const char* format, va_list args) { + if (is_color_enabled()) { + std::printf("%s", color_code); + } + + std::vprintf(format, args); + + if (is_color_enabled()) { + std::printf("%s", color::RESET); + } +} + +void printf_color(const char* color_code, const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color_code, format, args); + va_end(args); +} + +void printf_red(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::RED, format, args); + va_end(args); +} + +void printf_green(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::GREEN, format, args); + va_end(args); +} + +void printf_yellow(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::YELLOW, format, args); + va_end(args); +} + +void printf_blue(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::BLUE, format, args); + va_end(args); +} + +void printf_cyan(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::CYAN, format, args); + va_end(args); +} + +void printf_magenta(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::MAGENTA, format, args); + va_end(args); +} + +void printf_white(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::WHITE, format, args); + va_end(args); +} + +void log_info(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::BLUE, format, args); + va_end(args); +} + +void log_success(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::GREEN, format, args); + va_end(args); +} + +void log_warning(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::YELLOW, format, args); + va_end(args); +} + +void log_error(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::RED, format, args); + va_end(args); +} + +void log_debug(const char* format, ...) { + va_list args; + va_start(args, format); + vprintf_color(color::CYAN, format, args); + va_end(args); +} + +void log_info_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[INFO]%s [%s:%d] ", color::BLUE, color::RESET, file, line); + } else { + std::printf("[INFO] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +void log_success_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[SUCCESS]%s [%s:%d] ", color::GREEN, color::RESET, file, line); + } else { + std::printf("[SUCCESS] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +void log_warning_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[WARNING]%s [%s:%d] ", color::YELLOW, color::RESET, file, line); + } else { + std::printf("[WARNING] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +void log_error_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[ERROR]%s [%s:%d] ", color::RED, color::RESET, file, line); + } else { + std::printf("[ERROR] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +void log_debug_at(const char* file, int line, const char* format, ...) { + va_list args; + va_start(args, format); + if (is_color_enabled()) { + std::printf("%s[DEBUG]%s [%s:%d] ", color::CYAN, color::RESET, file, line); + } else { + std::printf("[DEBUG] [%s:%d] ", file, line); + } + std::vprintf(format, args); + va_end(args); +} + +} // namespace robot + diff --git a/src/Libraries/robot_cpp/src/node_handle.cpp b/src/Libraries/robot_cpp/src/node_handle.cpp new file mode 100644 index 0000000..748af17 --- /dev/null +++ b/src/Libraries/robot_cpp/src/node_handle.cpp @@ -0,0 +1,631 @@ +#include +#include +#include +#include +#include + +namespace robot +{ + +NodeHandle::NodeHandle() : namespace_("") +{ + node_handle_ = YAML::Node(); +} + +NodeHandle::NodeHandle(const std::string &name) : namespace_(name) +{ + node_handle_ = YAML::Node(); +} + +NodeHandle::~NodeHandle() +{ +} + +YAML::Node NodeHandle::getNestedValue(const std::string &key) const +{ + if (!node_handle_.IsDefined() || !node_handle_.IsMap()) + { + return YAML::Node(); + } + + // Split key by '/' to handle nested keys + std::stringstream ss(key); + std::string segment; + std::vector segments; + + while (std::getline(ss, segment, '/')) + { + if (!segment.empty()) + { + segments.push_back(segment); + } + } + + if (segments.empty()) + { + return node_handle_; + } + + YAML::Node current = node_handle_; + + for (size_t i = 0; i < segments.size(); ++i) + { + if (!current.IsMap()) + { + return YAML::Node(); + } + + if (!current[segments[i]].IsDefined()) + { + return YAML::Node(); + } + + current = current[segments[i]]; + } + + return current; +} + +YAML::Node NodeHandle::getParamValue(const std::string &key) const +{ + return getNestedValue(key); +} + +void NodeHandle::setParam(const std::string &key, const YAML::Node &value) +{ + if (!node_handle_.IsMap()) + { + node_handle_ = YAML::Node(YAML::NodeType::Map); + } + + // Split key by '/' to handle nested keys + std::stringstream ss(key); + std::string segment; + std::vector segments; + + while (std::getline(ss, segment, '/')) + { + if (!segment.empty()) + { + segments.push_back(segment); + } + } + + if (segments.empty()) + { + node_handle_ = value; + return; + } + + YAML::Node current = node_handle_; + + for (size_t i = 0; i < segments.size() - 1; ++i) + { + if (!current[segments[i]].IsDefined() || !current[segments[i]].IsMap()) + { + current[segments[i]] = YAML::Node(YAML::NodeType::Map); + } + current = current[segments[i]]; + } + + // Set the final value + current[segments.back()] = value; +} + +bool NodeHandle::loadYamlFile(const std::string &filepath) +{ + try + { + node_handle_ = YAML::LoadFile(filepath); + return true; + } + catch (const YAML::Exception &e) + { + return false; + } +} + +std::string NodeHandle::getNamespace() const +{ + return namespace_; +} + +void NodeHandle::setParam(const std::string &key, bool value) +{ + setParam(key, YAML::Node(value)); +} + +void NodeHandle::setParam(const std::string &key, int value) +{ + setParam(key, YAML::Node(value)); +} + +void NodeHandle::setParam(const std::string &key, double value) +{ + setParam(key, YAML::Node(value)); +} + +void NodeHandle::setParam(const std::string &key, float value) +{ + setParam(key, YAML::Node(static_cast(value))); +} + +void NodeHandle::setParam(const std::string &key, const std::string &value) +{ + setParam(key, YAML::Node(value)); +} + +bool NodeHandle::getParam(const std::string &key, bool &b, bool default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined()) + { + b = default_value; + return false; + } + + if (value.IsScalar()) + { + try + { + if (value.Type() == YAML::NodeType::Scalar) + { + std::string str = value.as(); + std::transform(str.begin(), str.end(), str.begin(), ::tolower); + if (str == "true" || str == "1" || str == "yes" || str == "on") + { + b = true; + return true; + } + else if (str == "false" || str == "0" || str == "no" || str == "off") + { + b = false; + return true; + } + } + b = value.as(); + return true; + } + catch (...) + { + b = default_value; + return false; + } + } + + b = default_value; + return false; +} + +bool NodeHandle::getParam(const std::string &key, double &d, double default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsScalar()) + { + d = default_value; + return false; + } + + try + { + d = value.as(); + return true; + } + catch (...) + { + d = default_value; + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, float &f, float default_value) const +{ + double d; + if (getParam(key, d, static_cast(default_value))) + { + f = static_cast(d); + return true; + } + f = default_value; + return false; +} + +bool NodeHandle::getParam(const std::string &key, int &i, int default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsScalar()) + { + i = default_value; + return false; + } + + try + { + std::string str = value.as(); + // Handle hex format + if (str.length() > 2 && str.substr(0, 2) == "0x") + { + i = std::stoi(str, nullptr, 16); + } + else + { + i = value.as(); + } + return true; + } + catch (...) + { + i = default_value; + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, std::string &s, std::string default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsScalar()) + { + s = default_value; + return false; + } + + try + { + s = value.as(); + return true; + } + catch (...) + { + s = default_value; + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsSequence()) + { + vec = default_value; + return false; + } + + vec.clear(); + try + { + for (size_t i = 0; i < value.size(); ++i) + { + if (value[i].IsScalar()) + { + try + { + bool b = value[i].as(); + vec.push_back(b); + } + catch (...) + { + // Try as string and convert + std::string str = value[i].as(); + std::transform(str.begin(), str.end(), str.begin(), ::tolower); + if (str == "true" || str == "1" || str == "yes" || str == "on") + { + vec.push_back(true); + } + else if (str == "false" || str == "0" || str == "no" || str == "off") + { + vec.push_back(false); + } + else + { + return false; + } + } + } + else + { + return false; + } + } + return true; + } + catch (...) + { + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsSequence()) + { + vec = default_value; + return false; + } + + vec.clear(); + try + { + for (size_t i = 0; i < value.size(); ++i) + { + vec.push_back(value[i].as()); + } + return true; + } + catch (...) + { + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const +{ + std::vector dvec; + std::vector ddefault; + ddefault.reserve(default_value.size()); + for (float f : default_value) + { + ddefault.push_back(static_cast(f)); + } + + if (getParam(key, dvec, ddefault)) + { + vec.clear(); + vec.reserve(dvec.size()); + for (double d : dvec) + { + vec.push_back(static_cast(d)); + } + return true; + } + vec = default_value; + return false; +} + +bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsSequence()) + { + vec = default_value; + return false; + } + + vec.clear(); + try + { + for (size_t i = 0; i < value.size(); ++i) + { + vec.push_back(value[i].as()); + } + return true; + } + catch (...) + { + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, std::vector &vec, std::vector default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsSequence()) + { + vec = default_value; + return false; + } + + vec.clear(); + try + { + for (size_t i = 0; i < value.size(); ++i) + { + vec.push_back(value[i].as()); + } + return true; + } + catch (...) + { + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsMap()) + { + map = default_value; + return false; + } + + map.clear(); + try + { + for (auto it = value.begin(); it != value.end(); ++it) + { + std::string key_str = it->first.as(); + if (it->second.IsScalar()) + { + try + { + bool b = it->second.as(); + map[key_str] = b; + } + catch (...) + { + // Try as string and convert + std::string str = it->second.as(); + std::transform(str.begin(), str.end(), str.begin(), ::tolower); + if (str == "true" || str == "1" || str == "yes" || str == "on") + { + map[key_str] = true; + } + else if (str == "false" || str == "0" || str == "no" || str == "off") + { + map[key_str] = false; + } + else + { + return false; + } + } + } + else + { + return false; + } + } + return true; + } + catch (...) + { + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsMap()) + { + map = default_value; + return false; + } + + map.clear(); + try + { + for (auto it = value.begin(); it != value.end(); ++it) + { + std::string key_str = it->first.as(); + if (it->second.IsScalar()) + { + map[key_str] = it->second.as(); + } + else + { + return false; + } + } + return true; + } + catch (...) + { + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const +{ + std::map dmap; + std::map ddefault; + for (const auto &pair : default_value) + { + ddefault[pair.first] = static_cast(pair.second); + } + + if (getParam(key, dmap, ddefault)) + { + map.clear(); + for (const auto &pair : dmap) + { + map[pair.first] = static_cast(pair.second); + } + return true; + } + map = default_value; + return false; +} + +bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsMap()) + { + map = default_value; + return false; + } + + map.clear(); + try + { + for (auto it = value.begin(); it != value.end(); ++it) + { + std::string key_str = it->first.as(); + if (it->second.IsScalar()) + { + map[key_str] = it->second.as(); + } + else + { + return false; + } + } + return true; + } + catch (...) + { + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, std::map &map, std::map default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined() || !value.IsMap()) + { + map = default_value; + return false; + } + + map.clear(); + try + { + for (auto it = value.begin(); it != value.end(); ++it) + { + std::string key_str = it->first.as(); + if (it->second.IsScalar()) + { + map[key_str] = it->second.as(); + } + else + { + return false; + } + } + return true; + } + catch (...) + { + return false; + } +} + +bool NodeHandle::getParam(const std::string &key, YAML::Node &v, YAML::Node default_value) const +{ + YAML::Node value = getNestedValue(key); + + if (!value.IsDefined()) + { + v = default_value; + return false; + } + + v = value; + return true; +} + +} // namespace robot diff --git a/src/Navigations/Cores/move_base_core/.gitignore b/src/Navigations/Cores/move_base_core/.gitignore new file mode 100755 index 0000000..d5dca0c --- /dev/null +++ b/src/Navigations/Cores/move_base_core/.gitignore @@ -0,0 +1,414 @@ +# ---> VisualStudio +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. +## +## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore + +# User-specific files +*.rsuser +*.suo +*.user +*.userosscache +*.sln.docstates + +# User-specific files (MonoDevelop/Xamarin Studio) +*.userprefs + +# Mono auto generated files +mono_crash.* + +# Build results +[Dd]ebug/ +[Dd]ebugPublic/ +[Rr]elease/ +[Rr]eleases/ +x64/ +x86/ +[Ww][Ii][Nn]32/ +[Aa][Rr][Mm]/ +[Aa][Rr][Mm]64/ +bld/ +[Bb]in/ +[Oo]bj/ +[Ll]og/ +[Ll]ogs/ + +# Visual Studio 2015/2017 cache/options directory +.vs/ +# Uncomment if you have tasks that create the project's static files in wwwroot +#wwwroot/ + +# Visual Studio 2017 auto generated files +Generated\ Files/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +# NUnit +*.VisualState.xml +TestResult.xml +nunit-*.xml + +# Build Results of an ATL Project +[Dd]ebugPS/ +[Rr]eleasePS/ +dlldata.c + +# Benchmark Results +BenchmarkDotNet.Artifacts/ + +# .NET Core +project.lock.json +project.fragment.lock.json +artifacts/ + +# ASP.NET Scaffolding +ScaffoldingReadMe.txt + +# StyleCop +StyleCopReport.xml + +# Files built by Visual Studio +*_i.c +*_p.c +*_h.h +*.ilk +*.meta +*.obj +*.iobj +*.pch +*.pdb +*.ipdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*_wpftmp.csproj +*.log +*.tlog +*.vspscc +*.vssscc +.builds +*.pidb +*.svclog +*.scc + +# Chutzpah Test files +_Chutzpah* + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opendb +*.opensdf +*.sdf +*.cachefile +*.VC.db +*.VC.VC.opendb + +# Visual Studio profiler +*.psess +*.vsp +*.vspx +*.sap + +# Visual Studio Trace Files +*.e2e + +# TFS 2012 Local Workspace +$tf/ + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper +*.DotSettings.user + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# AxoCover is a Code Coverage Tool +.axoCover/* +!.axoCover/settings.json + +# Coverlet is a free, cross platform Code Coverage Tool +coverage*.json +coverage*.xml +coverage*.info + +# Visual Studio code coverage results +*.coverage +*.coveragexml + +# NCrunch +_NCrunch_* +.*crunch*.local.xml +nCrunchTemp_* + +# MightyMoose +*.mm.* +AutoTest.Net/ + +# Web workbench (sass) +.sass-cache/ + +# Installshield output folder +[Ee]xpress/ + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish/ + +# Publish Web Output +*.[Pp]ublish.xml +*.azurePubxml +# Note: Comment the next line if you want to checkin your web deploy settings, +# but database connection strings (with potential passwords) will be unencrypted +*.pubxml +*.publishproj + +# Microsoft Azure Web App publish settings. Comment the next line if you want to +# checkin your Azure Web App publish settings, but sensitive information contained +# in these scripts will be unencrypted +PublishScripts/ + +# NuGet Packages +*.nupkg +# NuGet Symbol Packages +*.snupkg +# The packages folder can be ignored because of Package Restore +**/[Pp]ackages/* +# except build/, which is used as an MSBuild target. +!**/[Pp]ackages/build/ +# Uncomment if necessary however generally it will be regenerated when needed +#!**/[Pp]ackages/repositories.config +# NuGet v3's project.json files produces more ignorable files +*.nuget.props +*.nuget.targets + +# Microsoft Azure Build Output +csx/ +*.build.csdef + +# Microsoft Azure Emulator +ecf/ +rcf/ + +# Windows Store app package directories and files +AppPackages/ +BundleArtifacts/ +Package.StoreAssociation.xml +_pkginfo.txt +*.appx +*.appxbundle +*.appxupload + +# Visual Studio cache files +# files ending in .cache can be ignored +*.[Cc]ache +# but keep track of directories ending in .cache +!?*.[Cc]ache/ + +# Others +ClientBin/ +~$* +*~ +*.dbmdl +*.dbproj.schemaview +*.jfm +*.pfx +*.publishsettings +orleans.codegen.cs + +# Including strong name files can present a security risk +# (https://github.com/github/gitignore/pull/2483#issue-259490424) +#*.snk + +# Since there are multiple workflows, uncomment next line to ignore bower_components +# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) +#bower_components/ + +# RIA/Silverlight projects +Generated_Code/ + +# Backup & report files from converting an old project file +# to a newer Visual Studio version. Backup files are not needed, +# because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML +UpgradeLog*.htm +ServiceFabricBackup/ +*.rptproj.bak + +# SQL Server files +*.mdf +*.ldf +*.ndf + +# Business Intelligence projects +*.rdl.data +*.bim.layout +*.bim_*.settings +*.rptproj.rsuser +*- [Bb]ackup.rdl +*- [Bb]ackup ([0-9]).rdl +*- [Bb]ackup ([0-9][0-9]).rdl + +# Microsoft Fakes +FakesAssemblies/ + +# GhostDoc plugin setting file +*.GhostDoc.xml + +# Node.js Tools for Visual Studio +.ntvs_analysis.dat +node_modules/ + +# Visual Studio 6 build log +*.plg + +# Visual Studio 6 workspace options file +*.opt + +# Visual Studio 6 auto-generated workspace file (contains which files were open etc.) +*.vbw + +# Visual Studio 6 auto-generated project file (contains which files were open etc.) +*.vbp + +# Visual Studio 6 workspace and project file (working project files containing files to include in project) +*.dsw +*.dsp + +# Visual Studio 6 technical files +*.ncb +*.aps + +# Visual Studio LightSwitch build output +**/*.HTMLClient/GeneratedArtifacts +**/*.DesktopClient/GeneratedArtifacts +**/*.DesktopClient/ModelManifest.xml +**/*.Server/GeneratedArtifacts +**/*.Server/ModelManifest.xml +_Pvt_Extensions + +# Paket dependency manager +.paket/paket.exe +paket-files/ + +# FAKE - F# Make +.fake/ + +# CodeRush personal settings +.cr/personal + +# Python Tools for Visual Studio (PTVS) +__pycache__/ +*.pyc + +# Cake - Uncomment if you are using it +# tools/** +# !tools/packages.config + +# Tabs Studio +*.tss + +# Telerik's JustMock configuration file +*.jmconfig + +# BizTalk build output +*.btp.cs +*.btm.cs +*.odx.cs +*.xsd.cs + +# OpenCover UI analysis results +OpenCover/ + +# Azure Stream Analytics local run output +ASALocalRun/ + +# MSBuild Binary and Structured Log +*.binlog + +# NVidia Nsight GPU debugger configuration file +*.nvuser + +# MFractors (Xamarin productivity tool) working folder +.mfractor/ + +# Local History for Visual Studio +.localhistory/ + +# Visual Studio History (VSHistory) files +.vshistory/ + +# BeatPulse healthcheck temp database +healthchecksdb + +# Backup folder for Package Reference Convert tool in Visual Studio 2017 +MigrationBackup/ + +# Ionide (cross platform F# VS Code tools) working folder +.ionide/ + +# Fody - auto-generated XML schema +FodyWeavers.xsd + +# VS Code files for those working on multiple tools +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +*.code-workspace + +# Local History for Visual Studio Code +.history/ + +# Windows Installer files from build outputs +*.cab +*.msi +*.msix +*.msm +*.msp + +# JetBrains Rider +*.sln.iml + +# ---> VisualStudioCode +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +!.vscode/*.code-snippets + +# Local History for Visual Studio Code +.history/ + +# Built Visual Studio Code Extensions +*.vsix + diff --git a/src/Navigations/Cores/move_base_core/CMakeLists.txt b/src/Navigations/Cores/move_base_core/CMakeLists.txt new file mode 100644 index 0000000..0cd3225 --- /dev/null +++ b/src/Navigations/Cores/move_base_core/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.10) + +# Tên dự án +project(move_base_core VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Cho phép các project khác include được header của move_base_core +set(MOVE_BASE_CORE_INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/include + PARENT_SCOPE +) + +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +# Tìm tất cả header files +file(GLOB HEADERS "include/move_base_core/*.h") + +# Tạo INTERFACE library (header-only) +add_library(move_base_core INTERFACE) +target_link_libraries(move_base_core INTERFACE tf3 robot_time geometry_msgs) + +# Set include directories +target_include_directories(move_base_core + INTERFACE + $ + $ +) + +# Cài đặt header files +install(DIRECTORY include/move_base_core + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +# Cài đặt target +install(TARGETS move_base_core + EXPORT move_base_core-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +# Export targets +install(EXPORT move_base_core-targets + FILE move_base_core-targets.cmake + DESTINATION lib/cmake/move_base_core) + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME} (Header-Only)") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Headers found:") +foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") +endforeach() +message(STATUS "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/move_base_core/README.md b/src/Navigations/Cores/move_base_core/README.md new file mode 100755 index 0000000..9db2d2b --- /dev/null +++ b/src/Navigations/Cores/move_base_core/README.md @@ -0,0 +1,2 @@ +# move_base_core + diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/common.h b/src/Navigations/Cores/move_base_core/include/move_base_core/common.h new file mode 100644 index 0000000..b454a25 --- /dev/null +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/common.h @@ -0,0 +1,16 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2025, Locus Robotics + * All rights reserved. + * + */ +#ifndef _MOVE_BASE_CORE_COMMON_H_INCLUDED_ +#define _MOVE_BASE_CORE_COMMON_H_INCLUDED_ + +#include +#include + +using TFListenerPtr = std::shared_ptr; + +#endif // NAV_CORE2_COMMON_H diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h new file mode 100755 index 0000000..8d398d7 --- /dev/null +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -0,0 +1,282 @@ +#ifndef _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_ +#define _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_ + +#include + +// geometry msgs +#include +#include +#include +#include + +// tf +#include +#include +#include +#include +#include + +namespace move_base_core +{ + // Navigation states, including planning and controller status + enum class State + { + PENDING, + ACTIVE, + PREEMPTED, + SUCCEEDED, + ABORTED, + REJECTED, + PREEMPTING, + RECALLING, + RECALLED, + LOST, + PLANNING, + CONTROLLING, + CLEARING, + PAUSED + }; + + // Feedback structure to describe current navigation status + struct NavFeedback + { + State navigation_state; // Current navigation state + std::string feed_back_str; // Description or debug message + geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D + bool goal_checked; // Whether the current goal is verified + bool is_ready; // Robot is ready for commands + }; + + /** + * @brief Convert a State enum to its string representation. + * Useful for debugging/logging or displaying in UI. + * + * @param state Enum value of move_base_core::State + * @return std::string The corresponding string, e.g. "PENDING" + */ + inline std::string toString(move_base_core::State state) + { + using move_base_core::State; + switch (state) + { + case State::PENDING: + return "PENDING"; // Chờ xử lý + case State::ACTIVE: + return "ACTIVE"; // Đang hoạt động + case State::PREEMPTED: + return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới + case State::SUCCEEDED: + return "SUCCEEDED"; // Thành công + case State::ABORTED: + return "ABORTED"; // Bị lỗi + case State::REJECTED: + return "REJECTED"; // Từ chối bởi planner hoặc controller + case State::PREEMPTING: + return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu + case State::RECALLING: + return "RECALLING"; // Đang huỷ bỏ nội bộ + case State::RECALLED: + return "RECALLED"; // Đã được thu hồi + case State::LOST: + return "LOST"; // Mất trạng thái + case State::PLANNING: + return "PLANNING"; // Đang lập kế hoạch đường đi + case State::CONTROLLING: + return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan + case State::CLEARING: + return "CLEARING"; // Đang dọn dẹp bản đồ / costmap + case State::PAUSED: + return "PAUSED"; // Tạm dừng + default: + return "UNKNOWN_STATE"; // Không xác định + } + } + + /** + * @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction. + * + * This function calculates a new position by moving forward (or backward if negative) + * a certain `offset_distance` from the current position, following the given heading angle (theta). + * + * @param pose The original 2D pose (x, y, theta) in the local plane. + * @param frame_id The coordinate frame in which the output PoseStamped will be expressed. + * @param offset_distance The distance to offset along the heading direction, in meters. + * Positive moves forward, negative moves backward. + * @return A new PoseStamped offset from the input pose, in the given frame. + */ + inline geometry_msgs::PoseStamped offset_goal(const geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance) + { + geometry_msgs::PoseStamped goal; + // goal.header.frame_id = frame_id; + // goal.header.stamp = robot::Time::now(); + // goal.pose.position.x = pose.x + offset_distance * cos(pose.theta); + // goal.pose.position.y = pose.y + offset_distance * sin(pose.theta); + + // tf3::Quaternion q; + // q.setRPY(0, 0, pose.theta); + // goal.pose.orientation = tf3::convert(q); + return goal; + } + + /** + * @brief Overloaded version: creates an offset target pose from a given PoseStamped. + * + * This function extracts the 2D position and orientation (yaw) from the given PoseStamped, + * offsets it forward (or backward) along the current heading direction, + * and returns a new PoseStamped in the same frame. + * + * @param pose The original pose with full position and orientation. + * @param offset_distance Distance to offset along the current heading direction (in meters). + * @return A new PoseStamped offset from the original pose. + */ + inline geometry_msgs::PoseStamped offset_goal(const geometry_msgs::PoseStamped &pose, double offset_distance) + { + geometry_msgs::Pose2D pose2d; + pose2d.x = pose.pose.position.x; + pose2d.y = pose.pose.position.y; + // pose2d.theta = tf2::getYaw(pose.pose.orientation); + return offset_goal(pose2d, pose.header.frame_id, offset_distance); + } + + /** + * @class BaseNavigation + * @brief Abstract interface for robot navigation modules. + * + * Provides core methods for setting goals, moving, rotating, and handling motion control. + * All navigation logic must implement this interface. + */ + class BaseNavigation + { + public: + using Ptr = std::shared_ptr; + + virtual ~BaseNavigation() {} + + /** + * @brief Initialize the navigation system. + * @param tf Shared pointer to the TF listener or buffer. + */ + virtual void initialize(TFListenerPtr tf) = 0; + + /** + * @brief Set the robot's footprint (outline shape) in the global frame. + * This can be used for planning or collision checking. + * + * @param fprt A vector of points representing the robot's footprint polygon. + * The points should be ordered counter-clockwise. + * Example: + * + ^ Y + | + | P3(-0.3, 0.2) P2(0.3, 0.2) + | ●---------------● + | | | + | | Robot | (view Top ) + | | | + | ●---------------● + | P4(-0.3, -0.2) P1(0.3, -0.2) + +-------------------------------> X + + std::vector footprint; + 1. footprint.push_back(make_point(0.3, -0.2)); + 2. footprint.push_back(make_point(0.3, 0.2)); + 3. footprint.push_back(make_point(-0.3, 0.2)); + 4. footprint.push_back(make_point(-0.3, -0.2)); + */ + virtual void setRobotFootprint(const std::vector &fprt) = 0; + + /** + * @brief Send a goal for the robot to navigate to. + * @param goal Target pose in the global frame. + * @param xy_goal_tolerance Acceptable error in X/Y (meters). + * @param yaw_goal_tolerance Acceptable angular error (radians). + * @return True if goal was accepted and sent successfully. + */ + virtual bool moveTo(const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, + double yaw_goal_tolerance = 0.0) = 0; + + /** + * @brief Send a docking goal to a predefined marker (e.g. charger). + * @param maker Marker name or ID. + * @param goal Target pose for docking. + * @param xy_goal_tolerance Acceptable XY error (meters). + * @param yaw_goal_tolerance Acceptable heading error (radians). + * @return True if docking command succeeded. + */ + virtual bool dockTo(const std::string &maker, + const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, + double yaw_goal_tolerance = 0.0) = 0; + + /** + * @brief Move straight toward the target position (X-axis). + * @param goal Target pose. + * @param xy_goal_tolerance Acceptable positional error (meters). + * @return True if command issued successfully. + */ + virtual bool moveStraightTo(const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0) = 0; + + /** + * @brief Rotate in place to align with target orientation. + * @param goal Pose containing desired heading (only Z-axis used). + * @param yaw_goal_tolerance Acceptable angular error (radians). + * @return True if rotation command was sent successfully. + */ + virtual bool rotateTo(const geometry_msgs::PoseStamped &goal, + double yaw_goal_tolerance = 0.0) = 0; + + /** + * @brief Pause the robot's movement (e.g. during obstacle avoidance). + */ + virtual void pause() = 0; + + /** + * @brief Resume motion after a pause. + */ + virtual void resume() = 0; + + /** + * @brief Cancel the current goal and stop the robot. + */ + virtual void cancel() = 0; + + /** + * @brief Send limited linear velocity command. + * @param linear Linear velocity vector (x, y, z). + * @return True if the command was accepted. + */ + virtual bool setTwistLinear(const geometry_msgs::Vector3 &linear) = 0; + + /** + * @brief Send limited angular velocity command. + * @param angular Angular velocity vector (x, y, z). + * @return True if the command was accepted. + */ + virtual bool setTwistAngular(const geometry_msgs::Vector3 &angular) = 0; + + /** + * @brief Get the robot’s pose as a PoseStamped. + * @param pose Output parameter with the robot’s current pose. + * @return True if pose was successfully retrieved. + */ + virtual bool getRobotPose(geometry_msgs::PoseStamped &pose) = 0; + + /** + * @brief Get the robot’s pose as a 2D pose. + * @param pose Output parameter with the robot’s current 2D pose. + * @return True if pose was successfully retrieved. + */ + virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0; + + // Shared feedback data for navigation status tracking + std::shared_ptr nav_feedback_; + + protected: + BaseNavigation() = default; + }; + +} // namespace move_base_core + +#endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_ diff --git a/src/Navigations/Cores/nav_core/CHANGELOG.rst b/src/Navigations/Cores/nav_core/CHANGELOG.rst new file mode 100755 index 0000000..1817763 --- /dev/null +++ b/src/Navigations/Cores/nav_core/CHANGELOG.rst @@ -0,0 +1,112 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nav_core +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.17.3 (2023-01-10) +------------------- + +1.17.2 (2022-06-20) +------------------- + +1.17.1 (2020-08-27) +------------------- + +1.17.0 (2020-04-02) +------------------- +* Merge pull request `#982 `_ from ros-planning/noetic_prep + Noetic Migration +* increase required cmake version +* Contributors: Michael Ferguson + +1.16.6 (2020-03-18) +------------------- + +1.16.5 (2020-03-15) +------------------- + +1.16.4 (2020-03-04) +------------------- + +1.16.3 (2019-11-15) +------------------- + +1.16.2 (2018-07-31) +------------------- + +1.16.1 (2018-07-28) +------------------- + +1.16.0 (2018-07-25) +------------------- +* Switch to TF2 `#755 `_ +* unify parameter names between base_local_planner and dwa_local_planner + addresses parts of `#90 `_ +* fix param names of RotateRecovery, closes `#706 `_ +* Contributors: David V. Lu, Michael Ferguson, Vincent Rabaud + +1.15.2 (2018-03-22) +------------------- +* Merge pull request `#673 `_ from ros-planning/email_update_lunar + update maintainer email (lunar) +* Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy + Add myself as a maintainer. +* Contributors: Aaron Hoy, Michael Ferguson + +1.15.1 (2017-08-14) +------------------- + +1.15.0 (2017-08-07) +------------------- +* convert packages to format2 +* makePlan overload must return. +* Contributors: Eric Tappan, Mikael Arguedas + +1.14.0 (2016-05-20) +------------------- + +1.13.1 (2015-10-29) +------------------- +* Merge pull request `#338 `_ from mikeferguson/planner_review +* fix doxygen, couple style fixes +* Contributors: Michael Ferguson + +1.13.0 (2015-03-17) +------------------- +* adding makePlan function with returned cost to base_global_planner +* Contributors: phil0stine + +1.12.0 (2015-02-04) +------------------- +* update maintainer email +* Contributors: Michael Ferguson + +1.11.15 (2015-02-03) +-------------------- + +1.11.14 (2014-12-05) +-------------------- + +1.11.13 (2014-10-02) +-------------------- + +1.11.12 (2014-10-01) +-------------------- + +1.11.11 (2014-07-23) +-------------------- + +1.11.10 (2014-06-25) +-------------------- + +1.11.9 (2014-06-10) +------------------- + +1.11.8 (2014-05-21) +------------------- + +1.11.7 (2014-05-21) +------------------- + +1.11.4 (2013-09-27) +------------------- +* Package URL Updates diff --git a/src/Navigations/Cores/nav_core/CMakeLists.txt b/src/Navigations/Cores/nav_core/CMakeLists.txt new file mode 100644 index 0000000..2a262de --- /dev/null +++ b/src/Navigations/Cores/nav_core/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.10) + +# Tên dự án +project(nav_core VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Cho phép các project khác include được header của nav_core +set(nav_core_INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/include + PARENT_SCOPE +) + +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +# Tạo INTERFACE library (header-only) +add_library(nav_core INTERFACE) +target_link_libraries(nav_core INTERFACE costmap_2d tf3) + +# Set include directories +target_include_directories(nav_core + INTERFACE + $ + $ +) + +# Cài đặt header files +install(DIRECTORY include/nav_core + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +install(TARGETS nav_core + EXPORT nav_core-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +# Export targets +install(EXPORT nav_core-targets + FILE nav_core-targets.cmake + DESTINATION lib/cmake/nav_core) + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME} (Header-Only)") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Headers found:") +foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") +endforeach() +message(STATUS "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h b/src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h new file mode 100755 index 0000000..7750bce --- /dev/null +++ b/src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h @@ -0,0 +1,96 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* 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 Willow Garage, Inc. 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. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ +#ifndef NAV_CORE_BASE_GLOBAL_PLANNER_H +#define NAV_CORE_BASE_GLOBAL_PLANNER_H + +#include +#include +#include + +namespace nav_core { + /** + * @class BaseGlobalPlanner + * @brief Provides an interface for global planners used in navigation. All global planners written as plugins for the navigation stack must adhere to this interface. + */ + class BaseGlobalPlanner{ + public: + using Ptr = std::shared_ptr; + + /** + * @brief Given a goal pose in the world, compute a plan + * @param start The start pose + * @param goal The goal pose + * @param plan The plan... filled by the planner + * @return True if a valid plan was found, false otherwise + */ + virtual bool makePlan(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, std::vector& plan) = 0; + + /** + * @brief Given a goal pose in the world, compute a plan + * @param start The start pose + * @param goal The goal pose + * @param plan The plan... filled by the planner + * @param cost The plans calculated cost + * @return True if a valid plan was found, false otherwise + */ + virtual bool makePlan(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, std::vector& plan, + double& cost) + { + cost = 0; + return makePlan(start, goal, plan); + } + + /** + * @brief Initialization function for the BaseGlobalPlanner + * @param name The name of this planner + * @param costmap_robot A pointer to the wrapper of the costmap to use for planning + */ + virtual bool initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) = 0; + + /** + * @brief Virtual destructor for the interface + */ + virtual ~BaseGlobalPlanner(){} + + protected: + BaseGlobalPlanner(){} + }; +} // namespace nav_core + +#endif // NAV_CORE_BASE_GLOBAL_PLANNER_H \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h b/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h new file mode 100755 index 0000000..1506514 --- /dev/null +++ b/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h @@ -0,0 +1,153 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Eitan Marder-Eppstein + *********************************************************************/ +#ifndef NAV_CORE_BASE_LOCAL_PLANNER_H +#define NAV_CORE_BASE_LOCAL_PLANNER_H + +#include +#include +#include +#include +#include + +namespace nav_core +{ + /** + * @class BaseLocalPlanner + * @brief Provides an interface for local planners used in navigation. All local planners written as plugins for the navigation stack must adhere to this interface. + */ + class BaseLocalPlanner + { + public: + using Ptr = std::shared_ptr; + /** + * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base + * @param cmd_vel Will be filled with the velocity command to be passed to the robot base + * @return True if a valid velocity command was found, false otherwise + */ + virtual bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) = 0; + + /** + * @brief Swap object will be injected interjace nav_core::BaseLocalPlanner + * @param planner_name The object name + * @return True if instance object is successed, False otherwise + */ + virtual bool swapPlanner(std::string planner_name) { return false; } + + /** + * @brief Set velocity for x, y asix of the robot + * @param linear the command velocity + * @return True if set is done, false otherwise + */ + virtual bool setTwistLinear(geometry_msgs::Vector3 linear) { return false; } + + /** + * @brief Get velocity for x, y asix of the robot + * @param linear The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); } + + /** + * @brief Set velocity theta for z asix of the robot + * @param angular the command velocity + * @return True if set is done, false otherwise + */ + virtual bool setTwistAngular(geometry_msgs::Vector3 angular) { return false; } + + /** + * @brief Get velocity theta for z asix of the robot + * @param direct The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); } + + /** + * @brief Get velocity for x, y asix of the robot + * @return The current velocity + */ + virtual geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); } + + /** + * @brief Check if the kinematic parameters are currently locked + * @return True if the kinematic parameters are locked, false otherwise + * + * This function indicates whether the kinematic parameters (e.g., velocities, accelerations, or direction settings) are in a locked state, preventing modifications. If the parameters are locked during in-place rotation, it could prevent updates to the direction or velocity, potentially causing inconsistencies in motion commands that lead to odometry errors and localization jumps in AMCL. + */ + virtual bool islock() {throw std::runtime_error("Function islock is not Support.");} + + /** + * @brief Lock the kinematic parameters to prevent modifications + */ + virtual void lock() {throw std::runtime_error("Function lock is not Support.");} + + /** + * @brief Unlock the kinematic parameters to allow modifications + */ + virtual void unlock() {throw std::runtime_error("Function unlock is not Support.");} + + /** + * @brief Check if the goal pose has been achieved by the local planner + * @return True if achieved, false otherwise + */ + virtual bool isGoalReached() = 0; + + /** + * @brief Set the plan that the local planner is following + * @param plan The plan to pass to the local planner + * @return True if the plan was updated successfully, false otherwise + */ + virtual bool setPlan(const std::vector &plan) = 0; + + /** + * @brief Constructs the local planner + * @param name The name to give this instance of the local planner + * @param tf A pointer to a transform listener + * @param costmap_ros The cost map to use for assigning costs to local plans + */ + virtual void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) = 0; + + /** + * @brief Virtual destructor for the interface + */ + virtual ~BaseLocalPlanner() {} + + protected: + BaseLocalPlanner() {} + }; +} // namespace nav_core + +#endif // NAV_CORE_BASE_LOCAL_PLANNER_H diff --git a/src/Navigations/Cores/nav_core/include/nav_core/parameter_magic.h b/src/Navigations/Cores/nav_core/include/nav_core/parameter_magic.h new file mode 100755 index 0000000..79889c1 --- /dev/null +++ b/src/Navigations/Cores/nav_core/include/nav_core/parameter_magic.h @@ -0,0 +1,86 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2018, Open Source Robotics Foundation +* 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 OSRF 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. +* +* Author: David Lu +*********************************************************************/ + +#ifndef NAV_CORE_PARAMETER_MAGIC_H +#define NAV_CORE_PARAMETER_MAGIC_H + +namespace nav_core +{ + +/** + * @brief Load a parameter from one of two namespaces. Complain if it uses the old name. + * @param nh NodeHandle to look for the parameter in + * @param current_name Parameter name that is current, i.e. not deprecated + * @param old_name Deprecated parameter name + * @param default_value If neither parameter is present, return this value + * @return The value of the parameter or the default value + */ +template +param_t loadParameterWithDeprecation(const std::string current_name, + const std::string old_name, const param_t& default_value) +{ + param_t value; + if (nh.hasParam(current_name)) + { + nh.getParam(current_name, value); + return value; + } + if (nh.hasParam(old_name)) + { + ROS_WARN("Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str()); + nh.getParam(old_name, value); + return value; + } + return default_value; +} + +/** + * @brief Warn if a parameter exists under a deprecated (and unsupported) name. + * + * Parameters loaded exclusively through dynamic reconfigure can't really use loadParamWithDeprecation. + */ +void warnRenamedParameter(const std::string current_name, const std::string old_name) +{ + if (nh.hasParam(old_name)) + { + ROS_WARN("Parameter %s is deprecated (and will not load properly). Use %s instead.", old_name.c_str(), current_name.c_str()); + } +} + +} // namespace nav_core + +#endif // NAV_CORE_PARAMETER_MAGIC_H diff --git a/src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h b/src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h new file mode 100755 index 0000000..c23f111 --- /dev/null +++ b/src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h @@ -0,0 +1,77 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* 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 Willow Garage, Inc. 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. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ +#ifndef NAV_CORE_RECOVERY_BEHAVIOR_H +#define NAV_CORE_RECOVERY_BEHAVIOR_H + +#include +#include +#include + +namespace nav_core { + /** + * @class RecoveryBehavior + * @brief Provides an interface for recovery behaviors used in navigation. All recovery behaviors written as plugins for the navigation stack must adhere to this interface. + */ + class RecoveryBehavior{ + public: + + using Ptr = std::shared_ptr; + + /** + * @brief Initialization function for the RecoveryBehavior + * @param tf A pointer to a transform listener + * @param global_costmap A pointer to the global_costmap used by the navigation stack + * @param local_costmap A pointer to the local_costmap used by the navigation stack + */ + virtual void initialize(std::string name, tf3::BufferCore* tf, costmap_2d::Costmap2DROBOT* global_costmap, costmap_2d::Costmap2DROBOT* local_costmap) = 0; + + /** + * @brief Runs the RecoveryBehavior + */ + virtual void runBehavior() = 0; + + /** + * @brief Virtual destructor for the interface + */ + virtual ~RecoveryBehavior(){} + + protected: + RecoveryBehavior(){} + }; +} // namespace nav_core + +#endif // NAV_CORE_RECOVERY_BEHAVIOR_H diff --git a/src/Navigations/Cores/nav_core2/CMakeLists.txt b/src/Navigations/Cores/nav_core2/CMakeLists.txt new file mode 100755 index 0000000..09cae9d --- /dev/null +++ b/src/Navigations/Cores/nav_core2/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.10) + +# Tên dự án +project(nav_core2 VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Cho phép các project khác include được header của nav_core2 +set(nav_core2_INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/include + PARENT_SCOPE +) + +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +# Tìm tất cả header files +file(GLOB HEADERS "include/nav_core2/*.h") + +# Tạo INTERFACE library (header-only) +add_library(nav_core2 INTERFACE) + +target_link_libraries(nav_core2 INTERFACE + costmap_2d + tf3 + nav_grid + nav_2d_msgs +) + +# Set include directories +target_include_directories(nav_core2 + INTERFACE + $ + $ +) + +# Cài đặt header files +install(DIRECTORY include/nav_core2 + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +# Cài đặt target +install(TARGETS nav_core2 + EXPORT nav_core2-targets) + +# Export targets +install(EXPORT nav_core2-targets + FILE nav_core2-targets.cmake + NAMESPACE nav_core2:: + DESTINATION lib/cmake/nav_core2) + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME} (Header-Only)") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Headers found:") +foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") +endforeach() +message(STATUS "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core2/README.md b/src/Navigations/Cores/nav_core2/README.md new file mode 100755 index 0000000..9e2ccd5 --- /dev/null +++ b/src/Navigations/Cores/nav_core2/README.md @@ -0,0 +1,52 @@ +# nav_core2 +A replacement interface for [nav_core](https://github.com/ros-planning/navigation/tree/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core) that defines basic two dimensional planner interfaces. + +There were a few key reasons for creating new interfaces rather than extending the old ones. + * Use `nav_2d_msgs` to eliminate unused data fields + * Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROS`. + * Provide more data in the interfaces for easier testing. + * Use Exceptions rather than booleans to provide more information about the types of errors encountered. + +## `Costmap` +`costmap_2d::Costmap2DROS` has been a vital part of the navigation stack for years, but it was not without its flaws. + * Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROS`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested. + * Initialization also started an update thread, which is also not always needed in testing. + * Using `Costmap2DROS` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation. + +The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `nav_grid::NavGrid` for abstracting away the data storage and coordinate manipulation, and provides a few other key methods for costmap functioning such as + * a mutex + * a way to potentially track changes to the costmap + * a public `update` method that can be called in whatever thread you please + +The `Costmap` can be loaded using `pluginlib`, allowing for arbitrary implementations of underlying update algorithms, include the layered costmap approach. + +One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROS`. + +Note: One could also imagine the `Costmap` interface being templatized itself like `NavGrid` instead of forcing use of `unsigned char`. While this may be possible, it was decided that it was a bit ambitious and the use of templates would force all of the implementation into headers, and would ultimately obfuscate the operation of algorithms. + +## Global Planner +Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_global_planner.h) to the new [nav_core2/GlobalPlanner](include/nav_core2/global_planner.h). + +| `nav_core` | `nav_core2` | comments | +|---|--|---| +|`void initialize(std::string, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener| +|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector&)`|`nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.| + +## Local Planner +Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_local_planner.h) to the new [nav_core2/LocalPlanner](include/nav_core2/local_planner.h). + +| `nav_core` | `nav_core2` | comments | +|---|--|---| +|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers| +|(no equivalent)|`void setGoalPose(const nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan` +|`bool setPlan(const std::vector&)`|`setPlan(const nav_2d_msgs::Path2D&)`|| +|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.| +|`bool isGoalReached()` | `bool isGoalReached(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. | + +## Exceptions +A hierarchical collection of [exceptions](include/nav_core2/exceptions.h) is provided to allow for reacting to navigation failures in a more robust and contextual way. +![exception hierarchy tree](doc/exceptions.png) +Each exception has a corresponding integer "result code" that can be used in ROS interfaces where passing the C++ object is infeasible. Note that due to the hierarchy, the result_code will be for the child-most exception. For example, if you throw a `StartBoundsException` which has a corresponding result code of `6`, it could also be seen as a `InvalidStartPoseException`, `GlobalPlannerException`, `PlannerException` or `NavCore2Exception`, all of which would also have the result code of `6`. + +## Bounds +For use in tracking `Costmap` changes and more, this package also provides an implementation of [bounding boxes](include/nav_core2/bounds.h). These are represented with the ranges `[min_x, max_x]` and `[min_y, max_y]` (inclusive). diff --git a/src/Navigations/Cores/nav_core2/doc/exceptions.png b/src/Navigations/Cores/nav_core2/doc/exceptions.png new file mode 100755 index 0000000..83f31e4 Binary files /dev/null and b/src/Navigations/Cores/nav_core2/doc/exceptions.png differ diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/basic_costmap.h b/src/Navigations/Cores/nav_core2/include/nav_core2/basic_costmap.h new file mode 100755 index 0000000..5018438 --- /dev/null +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/basic_costmap.h @@ -0,0 +1,68 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_CORE2_BASIC_COSTMAP_H +#define NAV_CORE2_BASIC_COSTMAP_H + +#include +#include +#include + +namespace nav_core2 +{ +class BasicCostmap : public nav_core2::Costmap +{ +public: + // Standard Costmap Interface + mutex_t* getMutex() override { return &my_mutex_; } + + // NavGrid Interface + void reset() override; + void setValue(const unsigned int x, const unsigned int y, const unsigned char& value) override; + unsigned char getValue(const unsigned int x, const unsigned int y) const override; + void setInfo(const nav_grid::NavGridInfo& new_info) override + { + info_ = new_info; + reset(); + } + + // Index Conversion + unsigned int getIndex(const unsigned int x, const unsigned int y) const; +protected: + mutex_t my_mutex_; + std::vector data_; +}; +} // namespace nav_core2 + +#endif // NAV_CORE2_BASIC_COSTMAP_H diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/bounds.h b/src/Navigations/Cores/nav_core2/include/nav_core2/bounds.h new file mode 100755 index 0000000..c3fcbaf --- /dev/null +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/bounds.h @@ -0,0 +1,209 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_CORE2_BOUNDS_H +#define NAV_CORE2_BOUNDS_H + +#include +#include +#include + +namespace nav_core2 +{ +/** + * @brief Templatized method for checking if a value falls inside a one-dimensional range + * @param value The value to check + * @param min_value The minimum value of the range + * @param max_value The maximum value of the range + * @return True if the value is within the range + */ +template +inline bool inRange(const NumericType value, const NumericType min_value, const NumericType max_value) +{ + return min_value <= value && value <= max_value; +} + +/** + * @class GenericBounds + * @brief Templatized class that represents a two dimensional bounds with ranges [min_x, max_x] [min_y, max_y] inclusive + */ +template +struct GenericBounds +{ +public: + /** + * @brief Constructor for an empty bounds + */ + GenericBounds() + { + reset(); + } + + /** + * @brief Constructor for a non-empty initial bounds + * @param x0 Initial min x + * @param y0 Initial min y + * @param x1 Initial max x + * @param y1 Initial max y + */ + GenericBounds(NumericType x0, NumericType y0, NumericType x1, NumericType y1) + : min_x_(x0), min_y_(y0), max_x_(x1), max_y_(y1) {} + + /** + * @brief Reset the bounds to be empty + */ + void reset() + { + min_x_ = min_y_ = std::numeric_limits::max(); + max_x_ = max_y_ = std::numeric_limits::lowest(); // -max + } + + /** + * @brief Update the bounds to include the point (x, y) + */ + void touch(NumericType x, NumericType y) + { + min_x_ = std::min(x, min_x_); + min_y_ = std::min(y, min_y_); + max_x_ = std::max(x, max_x_); + max_y_ = std::max(y, max_y_); + } + + /** + * @brief Update the bounds to include points (x0, y0) and (x1, y1) + * @param x0 smaller of two x values + * @param y0 smaller of two y values + * @param x1 larger of two x values + * @param y1 larger of two y values + */ + void update(NumericType x0, NumericType y0, NumericType x1, NumericType y1) + { + min_x_ = std::min(x0, min_x_); + min_y_ = std::min(y0, min_y_); + max_x_ = std::max(x1, max_x_); + max_y_ = std::max(y1, max_y_); + } + + /** + * @brief Update the bounds to include the entirety of another bounds object + * @param other Another bounds object + */ + void merge(const GenericBounds& other) + { + update(other.min_x_, other.min_y_, other.max_x_, other.max_y_); + } + + /** + * @brief Returns true if the range is empty + */ + bool isEmpty() const + { + return min_x_ > max_x_ && min_y_ > max_y_; + } + + /** + * @brief Returns true if the point is inside this range + */ + bool contains(NumericType x, NumericType y) const + { + return inRange(x, min_x_, max_x_) && inRange(y, min_y_, max_y_); + } + + /** + * @brief returns true if the two bounds overlap each other + */ + bool overlaps(const GenericBounds& other) const + { + return !isEmpty() && !other.isEmpty() + && max_y_ >= other.min_y_ && min_y_ <= other.max_y_ + && max_x_ >= other.min_x_ && min_x_ <= other.max_x_; + } + + /** + * @brief comparison operator that requires all fields are equal + */ + bool operator==(const GenericBounds& other) const + { + return min_x_ == other.min_x_ && min_y_ == other.min_y_ && + max_x_ == other.max_x_ && max_y_ == other.max_y_; + } + + bool operator!=(const GenericBounds& other) const + { + return !operator==(other); + } + + /** + * @brief Returns a string representation of the bounds + */ + std::string toString() const + { + if (!isEmpty()) + { + return "(" + std::to_string(min_x_) + "," + std::to_string(min_y_) + "):(" + + std::to_string(max_x_) + "," + std::to_string(max_y_) + ")"; + } + else + { + return "(empty bounds)"; + } + } + + NumericType getMinX() const { return min_x_; } + NumericType getMinY() const { return min_y_; } + NumericType getMaxX() const { return max_x_; } + NumericType getMaxY() const { return max_y_; } + +protected: + NumericType min_x_, min_y_, max_x_, max_y_; +}; + +using Bounds = GenericBounds; + +inline unsigned int getDimension(unsigned int min_v, unsigned int max_v) +{ + return (min_v > max_v) ? 0 : max_v - min_v + 1; +} + +class UIntBounds : public GenericBounds +{ +public: + using GenericBounds::GenericBounds; + unsigned int getWidth() const { return getDimension(min_x_, max_x_); } + unsigned int getHeight() const { return getDimension(min_y_, max_y_); } +}; + +} // namespace nav_core2 + +#endif // NAV_CORE2_BOUNDS_H diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/common.h b/src/Navigations/Cores/nav_core2/include/nav_core2/common.h new file mode 100755 index 0000000..c726589 --- /dev/null +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/common.h @@ -0,0 +1,42 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_CORE2_COMMON_H +#define NAV_CORE2_COMMON_H + +#include +#include + +using TFListenerPtr = std::shared_ptr; + +#endif // NAV_CORE2_COMMON_H diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h b/src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h new file mode 100755 index 0000000..8fde3f3 --- /dev/null +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h @@ -0,0 +1,157 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_CORE2_COSTMAP_H +#define NAV_CORE2_COSTMAP_H + +#include +#include +#include +#include +#include +#include +#include + +namespace nav_core2 +{ + +class Costmap : public nav_grid::NavGrid +{ +public: + static const unsigned char NO_INFORMATION = 255; + static const unsigned char LETHAL_OBSTACLE = 254; + static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; + static const unsigned char FREE_SPACE = 0; + + using Ptr = std::shared_ptr; + + /** + * @brief Virtual Destructor + */ + virtual ~Costmap() {} + + /** + * @brief Initialization function for the Costmap + * + * ROS parameters/topics are expected to be in the parent/name namespace. + * It is suggested that all NodeHandles in the costmap use the parent NodeHandle's callback queue. + * + * @param parent NodeHandle to derive other NodeHandles from + * @param name The namespace for the costmap + * @param tf A pointer to a transform listener + */ + virtual void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) {} + + inline unsigned char getCost(const unsigned int x, const unsigned int y) + { + return getValue(x, y); + } + + inline unsigned char getCost(const nav_grid::Index& index) + { + return getValue(index.x, index.y); + } + + inline void setCost(const unsigned int x, const unsigned int y, const unsigned char cost) + { + setValue(x, y, cost); + } + + inline void setCost(const nav_grid::Index& index, const unsigned char cost) + { + setValue(index, cost); + } + + /** + * @brief Update the values in the costmap + * + * Note that this method can throw CostmapExceptions to indicate problems, like when it would be unsafe to navigate. + * e.g. If the costmap expects laser data at a given rate, but laser data hasn't shown up in a while, this method + * might throw a CostmapDataLagException. + */ + virtual void update() {} + + using mutex_t = boost::recursive_mutex; + /** + * @brief Accessor for boost mutex + */ + virtual mutex_t* getMutex() = 0; + + /** + * @brief Flag to indicate whether this costmap is able to track how much has changed + */ + virtual bool canTrackChanges() { return false; } + + /** + * @brief If canTrackChanges, get the bounding box for how much of the costmap has changed + * + * Rather than querying based on time stamps (which can require an arbitrary amount of storage) + * we instead query based on a namespace. The return bounding box reports how much of the costmap + * has changed since the last time this method was called with a particular namespace. If a namespace + * is new, then it returns a bounding box for the whole costmap. The max values are inclusive. + * + * Example Sequence with a 5x5 costmap: (results listed (min_x,min_y):(max_x, max_y)) + * 0) getChangeBounds("A") --> (0,0):(4,4) + * 1) getChangeBounds("B") --> (0,0):(4,4) + * 2) update cell 1, 1 + * 3) getChangeBounds("C") --> (0,0):(4,4) + * 4) getChangeBounds("A") --> (1,1):(1,1) + * 5) getChangeBounds("A") --> (empty bounds) (i.e. nothing was updated since last call) + * 6) updateCell 2, 4 + * 7) getChangeBounds("A") --> (2,4):(2,4) + * 8) getChangeBounds("B") --> (1,1):(2,4) + * + * @param ns The namespace + * @return Updated bounds + * @throws std::runtime_error If canTrackChanges is false, the returned bounds would be meaningless + */ + virtual UIntBounds getChangeBounds(const std::string& ns) + { + if (!canTrackChanges()) + { + throw std::runtime_error("You called 'getChangeBounds()' on a derived Costmap type that is not capable of " + "tracking changes (i.e. canTrackChanges() returns false). You shouldn't do that."); + } + else + { + throw std::runtime_error("You called 'getChangeBounds()' on a derived Costmap type that is capable of tracking " + "changes but has not properly implemented this function. You should yell at the author " + "of the derived Costmap."); + } + return UIntBounds(); + } +}; +} // namespace nav_core2 + +#endif // NAV_CORE2_COSTMAP_H diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h b/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h new file mode 100755 index 0000000..df01ac3 --- /dev/null +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h @@ -0,0 +1,321 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_CORE2_EXCEPTIONS_H +#define NAV_CORE2_EXCEPTIONS_H + +#include +#include +#include +#include +#include + +/************************************************** + * The nav_core2 Planning Exception Hierarchy!! + * (with arbitrary integer result codes) + ************************************************** + * NavCore2Exception + * 0 CostmapException + * 1 CostmapSafetyException + * 2 CostmapDataLagException + * 3 PlannerException + * 4 GlobalPlannerException + * 5 InvalidStartPoseException + * 6 StartBoundsException + * 7 OccupiedStartException + * 8 InvalidGoalPoseException + * 9 GoalBoundsException + * 10 OccupiedGoalException + * 11 NoGlobalPathException + * 12 GlobalPlannerTimeoutException + * 13 LocalPlannerException + * 14 IllegalTrajectoryException + * 15 NoLegalTrajectoriesException + * 16 PlannerTFException + * + * -1 Unknown + **************************************************/ + +namespace nav_core2 +{ + +inline std::string poseToString(const nav_2d_msgs::Pose2DStamped& pose) +{ + return "(" + std::to_string(pose.pose.x) + ", " + std::to_string(pose.pose.y) + ", " + std::to_string(pose.pose.theta) + + " : " + pose.header.frame_id + ")"; +} + +class NavCore2Exception: public std::runtime_error +{ +public: + explicit NavCore2Exception(const std::string& description, int result_code) + : std::runtime_error(description), result_code_(result_code) {} + int getResultCode() const { return result_code_; } +protected: + int result_code_; +}; + +using NavCore2ExceptionPtr = std::exception_ptr; + +/** + * @brief Handy function for getting the result code + */ +inline int getResultCode(const NavCore2ExceptionPtr& e_ptr) +{ + if (e_ptr == nullptr) + { + return -1; + } + try + { + std::rethrow_exception(e_ptr); + } + catch (const NavCore2Exception& e) + { + return e.getResultCode(); + } + catch (...) + { + // Will end up here if current_exception returned a non-NavCore2Exception + return -1; + } +} + +/** + * @class CostmapException + * @brief Extensible exception class for all costmap-related problems + */ +class CostmapException: public NavCore2Exception +{ +public: + explicit CostmapException(const std::string& description, int result_code = 0) + : NavCore2Exception(description, result_code) {} +}; + +/** + * @class CostmapSafetyException + * @brief General container for exceptions thrown when the costmap thinks any movement would be unsafe + */ +class CostmapSafetyException: public CostmapException +{ +public: + explicit CostmapSafetyException(const std::string& description, int result_code = 1) + : CostmapException(description, result_code) {} +}; + +/** + * @class CostmapDataLagException + * @brief Indicates costmap is out of date because data in not up to date. + * + * Functions similarly to `!Costmap2DROS::isCurrent()` + */ +class CostmapDataLagException: public CostmapSafetyException +{ +public: + explicit CostmapDataLagException(const std::string& description, int result_code = 2) + : CostmapSafetyException(description, result_code) {} +}; + +/** + * @class PlannerException + * @brief Parent type of all exceptions defined within + */ +class PlannerException: public NavCore2Exception +{ +public: + explicit PlannerException(const std::string& description, int result_code = 3) + : NavCore2Exception(description, result_code) {} +}; + +/** + * @class GlobalPlannerException + * @brief General container for exceptions thrown from the Global Planner + */ +class GlobalPlannerException: public PlannerException +{ +public: + explicit GlobalPlannerException(const std::string& description, int result_code = 4) + : PlannerException(description, result_code) {} +}; + +/** + * @class LocalPlannerException + * @brief General container for exceptions thrown from the Local Planner + */ +class LocalPlannerException: public PlannerException +{ +public: + explicit LocalPlannerException(const std::string& description, int result_code = 13) + : PlannerException(description, result_code) {} +}; + +/** + * @class PlannerTFException + * @brief Thrown when either the global or local planner cannot complete its operation due to TF errors + */ +class PlannerTFException: public PlannerException +{ +public: + explicit PlannerTFException(const std::string& description, int result_code = 16) + : PlannerException(description, result_code) {} +}; + +/** + * @class InvalidStartPoseException + * @brief Exception thrown when there is a problem at the start location for the global planner + */ +class InvalidStartPoseException: public GlobalPlannerException +{ +public: + explicit InvalidStartPoseException(const std::string& description, int result_code = 5) + : GlobalPlannerException(description, result_code) {} + InvalidStartPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) : + InvalidStartPoseException("The starting pose " + poseToString(pose) + " is " + problem, result_code) {} +}; + +/** + * @class StartBoundsException + * @brief Exception thrown when the start location of the global planner is out of the expected bounds + */ +class StartBoundsException: public InvalidStartPoseException +{ +public: + explicit StartBoundsException(const std::string& description, int result_code = 6) + : InvalidStartPoseException(description, result_code) {} + explicit StartBoundsException(const nav_2d_msgs::Pose2DStamped& pose) : + InvalidStartPoseException(pose, "out of bounds", 6) {} +}; + +/** + * @class OccupiedStartException + * @brief Exception thrown when the start location of the global planner is occupied in the costmap + */ +class OccupiedStartException: public InvalidStartPoseException +{ +public: + explicit OccupiedStartException(const std::string& description, int result_code = 7) + : InvalidStartPoseException(description, result_code) {} + explicit OccupiedStartException(const nav_2d_msgs::Pose2DStamped& pose) : + InvalidStartPoseException(pose, "occupied", 7) {} +}; + +/** + * @class InvalidGoalPoseException + * @brief Exception thrown when there is a problem at the goal location for the global planner + */ +class InvalidGoalPoseException: public GlobalPlannerException +{ +public: + explicit InvalidGoalPoseException(const std::string& description, int result_code = 8) + : GlobalPlannerException(description, result_code) {} + InvalidGoalPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) : + GlobalPlannerException("The goal pose " + poseToString(pose) + " is " + problem, result_code) {} +}; + +/** + * @class GoalBoundsException + * @brief Exception thrown when the goal location of the global planner is out of the expected bounds + */ +class GoalBoundsException: public InvalidGoalPoseException +{ +public: + explicit GoalBoundsException(const std::string& description, int result_code = 9) + : InvalidGoalPoseException(description, result_code) {} + explicit GoalBoundsException(const nav_2d_msgs::Pose2DStamped& pose) : + InvalidGoalPoseException(pose, "out of bounds", 9) {} +}; + +/** + * @class OccupiedGoalException + * @brief Exception thrown when the goal location of the global planner is occupied in the costmap + */ +class OccupiedGoalException: public InvalidGoalPoseException +{ +public: + explicit OccupiedGoalException(const std::string& description, int result_code = 10) + : InvalidGoalPoseException(description, result_code) {} + explicit OccupiedGoalException(const nav_2d_msgs::Pose2DStamped& pose) : + InvalidGoalPoseException(pose, "occupied", 10) {} +}; + +/** + * @class NoGlobalPathException + * @brief Exception thrown when the global planner cannot find a path from the start to the goal + */ +class NoGlobalPathException: public GlobalPlannerException +{ +public: + explicit NoGlobalPathException(const std::string& description, int result_code = 11) + : GlobalPlannerException(description, result_code) {} + NoGlobalPathException() : GlobalPlannerException("No global path found.") {} +}; + +/** + * @class GlobalPlannerTimeoutException + * @brief Exception thrown when the global planner has spent too long looking for a path + */ +class GlobalPlannerTimeoutException: public GlobalPlannerException +{ +public: + explicit GlobalPlannerTimeoutException(const std::string& description, int result_code = 12) + : GlobalPlannerException(description, result_code) {} +}; + +/** + * @class IllegalTrajectoryException + * @brief Thrown when one of the critics encountered a fatal error + */ +class IllegalTrajectoryException: public LocalPlannerException +{ +public: + IllegalTrajectoryException(const std::string& critic_name, const std::string& description, int result_code = 14) + : LocalPlannerException(description, result_code), critic_name_(critic_name) {} + std::string getCriticName() const { return critic_name_; } +protected: + std::string critic_name_; +}; + +/** + * @class NoLegalTrajectoriesException + * @brief Thrown when all the trajectories explored are illegal + */ +class NoLegalTrajectoriesException: public LocalPlannerException +{ +public: + explicit NoLegalTrajectoriesException(const std::string& description, int result_code = 15) + : LocalPlannerException(description, result_code) {} +}; + +} // namespace nav_core2 + +#endif // NAV_CORE2_EXCEPTIONS_H diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h b/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h new file mode 100755 index 0000000..e5f5e69 --- /dev/null +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h @@ -0,0 +1,85 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_CORE2_GLOBAL_PLANNER_H +#define NAV_CORE2_GLOBAL_PLANNER_H + +#include +#include +#include +#include +#include +#include + +namespace nav_core2 +{ + +/** + * @class GlobalPlanner + * @brief Provides an interface for global planners used in navigation. + */ +class GlobalPlanner +{ +public: + using Ptr = std::shared_ptr; + /** + * @brief Virtual Destructor + */ + virtual ~GlobalPlanner() {} + + /** + * @brief Initialization function for the GlobalPlanner + * + * ROS parameters/topics are expected to be in the parent/name namespace. + * It is suggested that all NodeHandles in the planner use the parent NodeHandle's callback queue. + * + * @param parent NodeHandle to derive other NodeHandles from + * @param name The name of this planner + * @param tf A pointer to a transform listener + * @param costmap A pointer to the costmap + */ + virtual void initialize(const YAML::Node& parent, const std::string& name, + TFListenerPtr tf, Costmap::Ptr costmap) = 0; + + /** + * @brief Run the global planner to make a plan starting at the start and ending at the goal. + * @param start The starting pose of the robot + * @param goal The goal pose of the robot + * @return The sequence of poses to get from start to goal, if any + */ + virtual nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start, + const nav_2d_msgs::Pose2DStamped& goal) = 0; +}; +} // namespace nav_core2 + +#endif // NAV_CORE2_GLOBAL_PLANNER_H diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h b/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h new file mode 100755 index 0000000..1df51f6 --- /dev/null +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h @@ -0,0 +1,173 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_CORE2_LOCAL_PLANNER_H +#define NAV_CORE2_LOCAL_PLANNER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nav_core2 +{ + + /** + * @class LocalPlanner + * @brief Provides an interface for local planners used in navigation. + */ + class LocalPlanner + { + public: + + using Ptr = std::shared_ptr; + /** + * @brief Virtual Destructor + */ + virtual ~LocalPlanner() {} + + /** + * @brief Constructs the local planner + * + * ROS parameters/topics are expected to be in the parent/name namespace. + * It is suggested that all NodeHandles in the planner use the parent NodeHandle's callback queue. + * + * @param parent NodeHandle to derive other NodeHandles from + * @param name The name to give this instance of the local planner + * @param tf A pointer to a transform listener + * @param costmap A pointer to the costmap + */ + // virtual void initialize(YAML::Node &parent, const std::string &name, + // TFListenerPtr tf, Costmap::Ptr costmap) = 0; + + virtual void initialize(YAML::Node &parent, const std::string &name, + TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap) = 0; + + /** + * @brief Sets the global goal for this local planner. + * @param goal_pose The Goal Pose + */ + virtual void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) = 0; + + /** + * @brief Sets the global plan for this local planner. + * + * @param path The global plan + */ + virtual void setPlan(const nav_2d_msgs::Path2D &path) = 0; + + /** + * @brief Compute the best command given the current pose, velocity and goal + * + * Get the local plan, given an initial pose, velocity and goal pose. + * It is presumed that the global plan is already set. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @return The best computed velocity + */ + virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, + const nav_2d_msgs::Twist2D &velocity) = 0; + + /** + * @brief set velocity for x, y asix of the robot + * @param linear the velocity command + * @return True if set is done, false otherwise + */ + virtual bool setTwistLinear(geometry_msgs::Vector3 linear) { return true; } + + /** + * @brief get velocity for x, y asix of the robot + * @param linear The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); } + + /** + * @brief Set velocity theta for z asix of the robot + * @param angular the command velocity + * @return True if set is done, false otherwise + */ + virtual bool setTwistAngular(geometry_msgs::Vector3 angular) { return false; } + + /** + * @brief Get velocity theta for z asix of the robot + * @param direct The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); } + + /** + * @brief Get velocity for x, y asix of the robot + * @return The current velocity + */ + virtual geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); } + + /** + * @brief Check if the kinematic parameters are currently locked + * @return True if the kinematic parameters are locked, false otherwise + * + * This function indicates whether the kinematic parameters (e.g., velocities, accelerations, or direction settings) are in a locked state, preventing modifications. If the parameters are locked during in-place rotation, it could prevent updates to the direction or velocity, potentially causing inconsistencies in motion commands that lead to odometry errors and localization jumps in AMCL. + */ + virtual bool islock() {throw std::runtime_error("Function islock is not Support.");} + + /** + * @brief Lock the kinematic parameters to prevent modifications + */ + virtual void lock() {throw std::runtime_error("Function lock is not Support.");} + + /** + * @brief Unlock the kinematic parameters to allow modifications + */ + virtual void unlock() {throw std::runtime_error("Function unlock is not Support.");} + + /** + * @brief Check to see whether the robot has reached its goal + * + * This tests whether the robot has fully reached the goal, given the current pose and velocity. + * + * @param query_pose The pose to check, in local costmap coordinates. + * @param velocity Velocity to check + * @return True if the goal conditions have been met + */ + virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0; + }; +} // namespace nav_core2 + +#endif // NAV_CORE2_LOCAL_PLANNER_H diff --git a/src/Navigations/Cores/nav_core2/src/basic_costmap.cpp b/src/Navigations/Cores/nav_core2/src/basic_costmap.cpp new file mode 100755 index 0000000..64a272a --- /dev/null +++ b/src/Navigations/Cores/nav_core2/src/basic_costmap.cpp @@ -0,0 +1,60 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 + +namespace nav_core2 +{ + +void BasicCostmap::reset() +{ + data_.assign(info_.width * info_.height, this->default_value_); +} + +unsigned int BasicCostmap::getIndex(const unsigned int x, const unsigned int y) const +{ + return y * info_.width + x; +} + +unsigned char BasicCostmap::getValue(const unsigned int x, const unsigned int y) const +{ + return data_[getIndex(x, y)]; +} + +void BasicCostmap::setValue(const unsigned int x, const unsigned int y, const unsigned char& value) +{ + data_[getIndex(x, y)] = value; +} + +} // namespace nav_core2 diff --git a/src/Navigations/Cores/nav_core2/test/bounds_test.cpp b/src/Navigations/Cores/nav_core2/test/bounds_test.cpp new file mode 100755 index 0000000..c9f19fa --- /dev/null +++ b/src/Navigations/Cores/nav_core2/test/bounds_test.cpp @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 + +using nav_core2::Bounds; +using nav_core2::UIntBounds; + +TEST(Bounds, test_bounds_simple) +{ + Bounds b; + EXPECT_TRUE(b.isEmpty()); + + b.touch(5.0, 6.0); + EXPECT_EQ(5.0, b.getMinX()); + EXPECT_EQ(5.0, b.getMinX()); + EXPECT_EQ(5.0, b.getMaxX()); + EXPECT_EQ(6.0, b.getMinY()); + EXPECT_EQ(6.0, b.getMaxY()); + EXPECT_TRUE(b.contains(5.0, 6.0)); + EXPECT_FALSE(b.contains(5.5, 6.0)); + EXPECT_FALSE(b.contains(5.5, 4.0)); + EXPECT_FALSE(b.isEmpty()); + + Bounds b2 = b; + EXPECT_EQ(5.0, b2.getMinX()); + EXPECT_EQ(5.0, b2.getMaxX()); + EXPECT_EQ(6.0, b2.getMinY()); + EXPECT_EQ(6.0, b2.getMaxY()); + + b.reset(); + EXPECT_EQ(5.0, b2.getMinX()); + EXPECT_EQ(5.0, b2.getMaxX()); + EXPECT_EQ(6.0, b2.getMinY()); + EXPECT_EQ(6.0, b2.getMaxY()); + EXPECT_FALSE(b.contains(5.0, 6.0)); + EXPECT_FALSE(b.contains(5.5, 6.0)); + EXPECT_TRUE(b2.contains(5.0, 6.0)); + EXPECT_FALSE(b.contains(5.5, 6.0)); + EXPECT_TRUE(b.isEmpty()); + + Bounds b3; + b3.touch(1.0, 5.0); + b3.touch(4.0, 2.0); + EXPECT_TRUE(b3.contains(3.0, 3.0)); + EXPECT_FALSE(b3.contains(0.0, 3.0)); + EXPECT_FALSE(b3.contains(5.0, 3.0)); + EXPECT_FALSE(b3.contains(3.0, 6.0)); + EXPECT_FALSE(b3.contains(3.0, 1.0)); +} + +TEST(Bounds, test_dimensions) +{ + UIntBounds empty; + UIntBounds square(0, 0, 5, 5); + UIntBounds rectangle(1, 4, 3, 15); + EXPECT_EQ(empty.getWidth(), 0u); + EXPECT_EQ(empty.getHeight(), 0u); + + EXPECT_EQ(square.getWidth(), 6u); + EXPECT_EQ(square.getHeight(), 6u); + + EXPECT_EQ(rectangle.getWidth(), 3u); + EXPECT_EQ(rectangle.getHeight(), 12u); +} + +TEST(Bounds, test_bounds_overlap) +{ + UIntBounds b0(0, 0, 5, 5); + UIntBounds b1(0, 0, 5, 5); + UIntBounds b2(0, 0, 3, 3); + UIntBounds b3(3, 0, 4, 4); + UIntBounds b4(4, 0, 4, 4); + UIntBounds b5(1, 4, 3, 15); + UIntBounds b6(10, 10, 10, 10); + EXPECT_TRUE(b0.overlaps(b0)); + EXPECT_TRUE(b0.overlaps(b1)); + EXPECT_TRUE(b0.overlaps(b2)); + EXPECT_TRUE(b2.overlaps(b0)); + EXPECT_TRUE(b0.overlaps(b3)); + EXPECT_TRUE(b2.overlaps(b3)); + EXPECT_FALSE(b2.overlaps(b4)); + EXPECT_TRUE(b0.overlaps(b5)); + EXPECT_TRUE(b5.overlaps(b0)); + EXPECT_FALSE(b0.overlaps(b6)); + EXPECT_FALSE(b6.overlaps(b0)); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/Navigations/Cores/nav_core2/test/exception_test.cpp b/src/Navigations/Cores/nav_core2/test/exception_test.cpp new file mode 100755 index 0000000..26c3ef2 --- /dev/null +++ b/src/Navigations/Cores/nav_core2/test/exception_test.cpp @@ -0,0 +1,133 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Locus Robotics + * 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 + +TEST(Exceptions, direct_code_access) +{ + // Make sure the caught exceptions have the same types as the thrown exception + // (This version does not create any copies of the exception) + try + { + throw nav_core2::GlobalPlannerTimeoutException("test case"); + } + catch (nav_core2::GlobalPlannerTimeoutException& x) + { + EXPECT_EQ(x.getResultCode(), 12); + } + + try + { + throw nav_core2::GlobalPlannerTimeoutException("test case"); + } + catch (nav_core2::PlannerException& x) + { + EXPECT_EQ(x.getResultCode(), 12); + } +} + +TEST(Exceptions, indirect_code_access) +{ + // Make sure the caught exceptions have the same types as the thrown exception + // (This version copies the exception to a new object with the parent type) + nav_core2::PlannerException e(""); + try + { + throw nav_core2::GlobalPlannerTimeoutException("test case"); + } + catch (nav_core2::GlobalPlannerTimeoutException& x) + { + e = x; + } + EXPECT_EQ(e.getResultCode(), 12); +} + +TEST(Exceptions, rethrow) +{ + // This version tests the ability to catch specific exceptions when rethrown + // A copy of the exception is made and rethrown, with the goal being able to catch the specific type + // and not the parent type. + nav_core2::NavCore2ExceptionPtr e; + try + { + throw nav_core2::GlobalPlannerTimeoutException("test case"); + } + catch (nav_core2::GlobalPlannerTimeoutException& x) + { + e = std::current_exception(); + } + + EXPECT_EQ(nav_core2::getResultCode(e), 12); + + try + { + std::rethrow_exception(e); + } + catch (nav_core2::GlobalPlannerTimeoutException& x) + { + EXPECT_EQ(x.getResultCode(), 12); + SUCCEED(); + } + catch (nav_core2::PlannerException& x) + { + FAIL() << "PlannerException caught instead of specific exception"; + } +} + +TEST(Exceptions, weird_exception) +{ + nav_core2::NavCore2ExceptionPtr e; + + // Check what happens with no exception + EXPECT_EQ(nav_core2::getResultCode(e), -1); + + // Check what happens with a non NavCore2Exception + try + { + std::string().at(1); // this generates an std::out_of_range + } + catch (...) + { + e = std::current_exception(); + } + + EXPECT_EQ(nav_core2::getResultCode(e), -1); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt new file mode 100755 index 0000000..3f46002 --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.10) + +# Tên dự án +project(nav_core_adapter VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 nav_2d_utils pthread) + +find_package(Boost REQUIRED COMPONENTS filesystem system) + +# Thư mục include +include_directories( + ${PROJECT_SOURCE_DIR}/include +) +# Tạo thư viện shared (.so) +add_library(costmap_adapter src/costmap_adapter.cpp) +target_link_libraries(costmap_adapter PRIVATE ${PACKAGES_DIR}) +target_include_directories(costmap_adapter PUBLIC + $ + $) + +add_library(local_planner_adapter src/local_planner_adapter.cpp) +target_link_libraries(local_planner_adapter + PRIVATE + Boost::filesystem + Boost::system + dl + costmap_adapter + ${PACKAGES_DIR} +) +target_include_directories(local_planner_adapter PRIVATE + $ + $) + +add_library(global_planner_adapter src/global_planner_adapter.cpp) +target_link_libraries(global_planner_adapter + PRIVATE + Boost::filesystem + Boost::system + dl + costmap_adapter + ${PACKAGES_DIR} +) + +target_include_directories(global_planner_adapter PRIVATE + $ + $) + +# Cài đặt header files +install(DIRECTORY include/nav_core_adapter + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +# Cài đặt library +install(TARGETS global_planner_adapter local_planner_adapter costmap_adapter + EXPORT nav_core_adapter-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +# Export targets +install(EXPORT nav_core_adapter-targets + FILE nav_core_adapter-targets.cmake + DESTINATION lib/cmake/nav_core_adapter) + +# Tùy chọn build +option(BUILD_SHARED_LIBS "Build shared libraries" ON) +option(BUILD_TESTS "Build test programs" OFF) + +# Flags biên dịch +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Headers found:") +foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") +endforeach() +message(STATUS "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core_adapter/README.md b/src/Navigations/Cores/nav_core_adapter/README.md new file mode 100755 index 0000000..38c067c --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/README.md @@ -0,0 +1,48 @@ +# nav_core_adapter +This package contains adapters for using `nav_core` plugins as `nav_core2` plugins and vice versa (more or less). In general, the adaptation process involves + * Converting between 2d and 3d datatypes. + * Converting between returning false and throwing exceptions on failure. + +We also provide an adapter for using a `costmap_2d::Costmap2DROS` as a plugin for the `nav_core2::Costmap` interface. + +## Adapter Classes + * Global Planner Adapters + * `GlobalPlannerAdapter` is used for employing a `nav_core2` global planner interface (such as `dlux_global_planner`) as a `nav_core` plugin, like in `move_base`. + * `GlobalPlannerAdapter2` is used for employing a `nav_core` global planner interface (such as `navfn`) +as a `nav_core2` plugin, like in `locomotor`. + * Local Planner Adapter + * `LocalPlannerAdapter` is used for employing a `nav_core2` local planner interface (such as `dwb_local_planner`) as a `nav_core` plugin, like in `move_base`. In addition to the standard adaptation steps listed above, the local planner adapter also uses the costmap to grab the global pose and subscribes to the odometry in order to get the current velocity. + * There is no `LocalPlannerAdapter2`. The `nav_core2` interfaces use additional information (like velocity) in the `local_planner` interface than its `nav_core` counterpart. This information would be ignored by a `nav_core` planner, so no adapter is provided. + * `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROS` object. It is not a perfect adaptation, because + * `Costmap2DROS` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though. + * `setInfo` is not implemented. + +## Parameter Setup +Let's look at a practical example of how to use `dwb_local_planner` in `move_base`. + +If you were using `DWA` you would probably have parameters set up like this: +``` +base_local_planner: dwa_local_planner/DWALocalPlanner +DWALocalPlanner: + acc_lim_x: 0.42 + ... +``` +i.e. you specify + * The name of the planner + * A bunch of additional parameters within the planner's namespace + +To use the adapter, you have to provide additional information. +``` +base_local_planner: nav_core_adapter::LocalPlannerAdapter +LocalPlannerAdapter: + planner_name: dwb_local_planner::DWBLocalPlanner +DWBLocalPlanner: + acc_lim_x: 0.42 + ... +``` +i.e. + * The name of the planner now points at the adapter + * The name of the actual planner loaded into the adapter's namespace + * The planner's parameters still in the planner's namespace. + +The process for the global planners is similar. diff --git a/src/Navigations/Cores/nav_core_adapter/cfg/NavCoreAdapter.cfg b/src/Navigations/Cores/nav_core_adapter/cfg/NavCoreAdapter.cfg new file mode 100755 index 0000000..55065e6 --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/cfg/NavCoreAdapter.cfg @@ -0,0 +1,9 @@ +#!/usr/bin/env python + +PACKAGE = 'nav_core_adapter' + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t + +gen = ParameterGenerator() +gen.add("planner_name", str_t, 0, "The name of the plugin for the local planner to use with move_base.", "base_local_planner/TrajectoryPlannerROS") +exit(gen.generate(PACKAGE, "nav_core_adapter", "NavCoreAdapter")) diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h new file mode 100755 index 0000000..ba612e9 --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h @@ -0,0 +1,85 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H +#define NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H + +#include +#include +#include +#include + +namespace nav_core_adapter +{ +nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot); + +class CostmapAdapter : public nav_core2::Costmap +{ +public: + /** + * @brief Deconstructor for possibly freeing the costmap_ros_ object + */ + virtual ~CostmapAdapter(); + + /** + * @brief Initialize from an existing Costmap2DROS object + * @param costmap_ros A Costmap2DROS object + * @param needs_destruction Whether to free the costmap_ros object when this class is destroyed + */ + void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false); + + // Standard Costmap Interface + void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) override; + nav_core2::Costmap::mutex_t* getMutex() override; + + // NavGrid Interface + void reset() override; + void update() override; + void setValue(const unsigned int x, const unsigned int y, const unsigned char& value) override; + unsigned char getValue(const unsigned int x, const unsigned int y) const override; + void setInfo(const nav_grid::NavGridInfo& new_info) override; + void updateInfo(const nav_grid::NavGridInfo& new_info) override; + + // Get Costmap Pointer for Backwards Compatibility + costmap_2d::Costmap2DROBOT* getCostmap2DROBOT() const { return costmap_robot_; } + +protected: + costmap_2d::Costmap2DROBOT* costmap_robot_; + costmap_2d::Costmap2D* costmap_; + bool needs_destruction_; +}; + +} // namespace nav_core_adapter + +#endif // NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h new file mode 100755 index 0000000..2cad914 --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h @@ -0,0 +1,76 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H +#define NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H + +#include +#include +// #include +#include +#include +#include + +namespace nav_core_adapter +{ + +/** + * @class GlobalPlannerAdapter + * @brief used for employing a `nav_core` global planner (such as `navfn`) as a `nav_core2` plugin, like in `locomotor`. + */ +class GlobalPlannerAdapter: public nav_core2::GlobalPlanner +{ +public: + GlobalPlannerAdapter(); + + // Nav Core 2 Global Planner Interface + void initialize(const YAML::Node& parent, const std::string& name, + TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; + nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start, + const nav_2d_msgs::Pose2DStamped& goal) override; + + static nav_core2::GlobalPlanner::Ptr create(); + +protected: + // Plugin handling + boost::function planner_loader_; + nav_core::BaseGlobalPlanner::Ptr planner_; + + costmap_2d::Costmap2DROBOT* costmap_robot_; + nav_core2::Costmap::Ptr costmap_; +}; + +} // namespace nav_core_adapter + +#endif // NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h new file mode 100755 index 0000000..093acce --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h @@ -0,0 +1,210 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H +#define NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nav_core_adapter +{ + struct NavCoreAdapterConfig + { + std::string planner_name; + bool use_odom; + bool use_costmap; + bool use_tf; + bool use_dynamic_reconfigure; + }; + + /** + * @class LocalPlannerAdapter + * @brief used for employing a nav_core2 local planner (such as dwb) as a nav_core plugin, like in move_base. + */ + class LocalPlannerAdapter : public nav_core::BaseLocalPlanner + { + public: + + using Ptr = boost::shared_ptr; + + /** + * @brief Constructor + */ + LocalPlannerAdapter(); + + /** + * @brief Destructor + */ + virtual ~LocalPlannerAdapter(); + + // Standard ROS Local Planner Interface + void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) override; + + /** + * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base + * @param cmd_vel Will be filled with the velocity command to be passed to the robot base + * @return True if a valid velocity command was found, false otherwise + */ + bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) override; + + /** + * @brief Swap object will be injected interjace nav_core::BaseLocalPlanner + * @param planner_name The object name + * @return True if instance object is successed, False otherwise + */ + virtual bool swapPlanner(std::string planner_name) override; + + /** + * @brief set velocity for x, y asix of the robot + * @param linear The velocity command + * @return True if set is done, false otherwise + */ + virtual bool setTwistLinear(geometry_msgs::Vector3 linear) override; + + /** + * @brief get velocity for x, y asix of the robot + * @param linear The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override; + + /** + * @brief Set velocity theta for z asix of the robot + * @param angular the command velocity + * @return True if set is done, false otherwise + */ + virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override; + + /** + * @brief Get velocity theta for z asix of the robot + * @param direct The velocity command + * @return True if set is done, false otherwise + */ + virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override; + + /** + * @brief Get actual velocity for x, y asix of the robot + * @return The current velocity + */ + virtual geometry_msgs::Twist getActualTwist() override; + + /** + * @brief Check if the kinematic parameters are currently locked + * @return True if the kinematic parameters are locked, false otherwise + * + * This function indicates whether the kinematic parameters (e.g., velocities, accelerations, or direction settings) are in a locked state, preventing modifications. If the parameters are locked during in-place rotation, it could prevent updates to the direction or velocity, potentially causing inconsistencies in motion commands that lead to odometry errors and localization jumps in AMCL. + */ + virtual bool islock() override; + + /** + * @brief Lock the kinematic parameters to prevent modifications + */ + virtual void lock() override; + + /** + * @brief Unlock the kinematic parameters to allow modifications + */ + virtual void unlock() override; + + /** + * @brief Check if the goal pose has been achieved by the local planner + * @return True if achieved, false otherwise + */ + bool isGoalReached() override; + + /** + * @brief Set the plan that the local planner is following + * @param plan The plan to pass to the local planner + * @return True if the plan was updated successfully, false otherwise + */ + bool setPlan(const std::vector &plan) override; + + /** + * @brief Create a new LocalPlannerAdapter + * @return A shared pointer to the new LocalPlannerAdapter + */ + static nav_core::BaseLocalPlanner::Ptr create(); + + protected: + /** + * @brief Get the robot pose from the costmap and store as Pose2DStamped + */ + bool getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d); + + /** + * @brief See if the back of the global plan matches the most recent goal pose + * @return True if the plan has changed + */ + bool hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal); + + // The most recent goal pose + nav_2d_msgs::Pose2DStamped last_goal_; + bool has_active_goal_; + + /** + * @brief helper class for subscribing to odometry + */ + std::shared_ptr odom_sub_; + + // Plugin handling + boost::function planner_loader_; + nav_core2::LocalPlanner::Ptr planner_; + + // Pointer Copies + TFListenerPtr tf_; + + std::shared_ptr costmap_adapter_; + costmap_2d::Costmap2DROBOT *costmap_robot_; + + boost::recursive_mutex configuration_mutex_; + // std::shared_ptr> dsrv_; + + YAML::Node private_nh_; + YAML::Node adapter_nh_; + // void reconfigureCB(nav_core_adapter::NavCoreAdapterConfig &config, uint32_t level); + nav_core_adapter::NavCoreAdapterConfig last_config_; + nav_core_adapter::NavCoreAdapterConfig default_config_; + bool setup_; + }; + +} // namespace nav_core_adapter + +#endif // NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/shared_pointers.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/shared_pointers.h new file mode 100755 index 0000000..d7b4d52 --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/shared_pointers.h @@ -0,0 +1,67 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_CORE_ADAPTER_SHARED_POINTERS_H +#define NAV_CORE_ADAPTER_SHARED_POINTERS_H + +#include +#include + +namespace nav_core_adapter +{ + +template +void null_deleter(T* raw_ptr) {} + +/** + * @brief Custom Constructor for creating a shared pointer to an existing object that doesn't delete the ptr when done + * + * @note This is considered bad form, and is only done here for backwards compatibility purposes. The nav_core2 + * interfaces require shared pointers, but the creation of the shared pointer from a raw pointer takes ownership + * of the object such that when the object containing the shared pointer is freed, the object pointed at by the + * shared pointer is also freed. This presents a problem, for instance, when switching from one planner to another + * if the costmap is freed by one planner. + * + * @param raw_ptr The raw pointer to an object + * @return A Shared pointer pointing at the raw_ptr, but when it is freed, the raw_ptr remains valid + */ +template +std::shared_ptr createSharedPointerWithNoDelete(T* raw_ptr) +{ + return std::shared_ptr(raw_ptr, null_deleter); +} + +} // namespace nav_core_adapter + +#endif // NAV_CORE_ADAPTER_SHARED_POINTERS_H diff --git a/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp new file mode 100755 index 0000000..a6ed199 --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp @@ -0,0 +1,119 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 + +// PLUGINLIB_EXPORT_CLASS(nav_core_adapter::CostmapAdapter, nav_core2::Costmap) + +namespace nav_core_adapter +{ + +nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot) +{ + nav_grid::NavGridInfo info; + costmap_2d::Costmap2D* costmap = costmap_robot->getCostmap(); + info.width = costmap->getSizeInCellsX(); + info.height = costmap->getSizeInCellsY(); + info.resolution = costmap->getResolution(); + info.frame_id = costmap_robot->getGlobalFrameID(); + info.origin_x = costmap->getOriginX(); + info.origin_y = costmap->getOriginY(); + return info; +} + +CostmapAdapter::~CostmapAdapter() +{ + if (needs_destruction_) + { + delete costmap_robot_; + } +} + +void CostmapAdapter::initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction) +{ + costmap_robot_ = costmap_robot; + needs_destruction_ = needs_destruction; + info_ = infoFromCostmap(costmap_robot_); + costmap_ = costmap_robot_->getCostmap(); +} + +void CostmapAdapter::initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) +{ + // TODO: Implement this if needed + throw nav_core2::CostmapException("initialize with YAML::Node not implemented"); +} + +nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex() +{ + return costmap_->getMutex(); +} + +void CostmapAdapter::reset() +{ + costmap_robot_->resetLayers(); +} + +void CostmapAdapter::update() +{ + info_ = infoFromCostmap(costmap_robot_); + if (!costmap_robot_->isCurrent()) + throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); +} + +void CostmapAdapter::setValue(const unsigned int x, const unsigned int y, const unsigned char& value) +{ + costmap_->setCost(x, y, value); +} + +unsigned char CostmapAdapter::getValue(const unsigned int x, const unsigned int y) const +{ + unsigned int index = costmap_->getIndex(x, y); + return costmap_->getCharMap()[index]; +} + +void CostmapAdapter::setInfo(const nav_grid::NavGridInfo& new_info) +{ + throw nav_core2::CostmapException("setInfo not implemented on CostmapAdapter"); +} + +void CostmapAdapter::updateInfo(const nav_grid::NavGridInfo& new_info) +{ + costmap_->updateOrigin(new_info.origin_x, new_info.origin_y); +} + +} // namespace nav_core_adapter diff --git a/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp new file mode 100755 index 0000000..d2c13a9 --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp @@ -0,0 +1,102 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 +#include +#include +#include +#include + +namespace nav_core_adapter +{ + + GlobalPlannerAdapter::GlobalPlannerAdapter() + // : planner_loader_("nav_core", "nav_core::BaseGlobalPlanner") + { + } + + /** + * @brief Load the nav_core global planner and initialize it + */ + void GlobalPlannerAdapter::initialize(const YAML::Node& parent, const std::string& name, + TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) + { + costmap_ = costmap; + std::shared_ptr ptr = std::static_pointer_cast(costmap); + + if (!ptr) + { + + std::cerr << "GlobalPlannerAdapter can only be used with the CostmapAdapter, not other Costmaps!" << std::endl; + exit(EXIT_FAILURE); + } + costmap_robot_ = ptr->getCostmap2DROBOT(); + + // YAML::Node planner_nh(parent, name); + std::string planner_name; + // planner_nh.param("planner_name", planner_name, std::string("global_planner/GlobalPlanner")); + // ROS_INFO_NAMED("GlobalPlannerAdapter", "Loading plugin %s", planner_name.c_str()); + std::string path_file_so; + planner_loader_ = boost::dll::import_alias( + path_file_so, planner_name, boost::dll::load_mode::append_decorations); + planner_ = planner_loader_(); + planner_->initialize(planner_name, costmap_robot_); + } + + nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const nav_2d_msgs::Pose2DStamped& start, + const nav_2d_msgs::Pose2DStamped& goal) + { + geometry_msgs::PoseStamped start3d = nav_2d_utils::pose2DToPoseStamped(start), + goal3d = nav_2d_utils::pose2DToPoseStamped(goal); + std::vector plan; + bool ret = planner_->makePlan(start3d, goal3d, plan); + if (!ret) + { + throw nav_core2::PlannerException("Generic Global Planner Error"); + } + return nav_2d_utils::posesToPath2D(plan); + } + + nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create() + { + return std::make_shared(); + } +} // namespace nav_core_adapter + +// register this planner as a GlobalPlanner plugin +BOOST_DLL_ALIAS(nav_core_adapter::GlobalPlannerAdapter::create, create_plugin) diff --git a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp new file mode 100755 index 0000000..e8ee33d --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp @@ -0,0 +1,408 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 +#include +#include +#include +#include +#include +#include + +namespace nav_core_adapter +{ + + LocalPlannerAdapter::LocalPlannerAdapter() : has_active_goal_(false) + { + } + + LocalPlannerAdapter::~LocalPlannerAdapter() + { + std::cout << "=== LocalPlannerAdapter destructor called ===" << std::endl; + + if (planner_) + { + std::cout << "Planner use_count before reset: " << planner_.use_count() << std::endl; + planner_.reset(); + std::cout << "Planner reset complete" << std::endl; + } + else + { + std::cout << "Planner already null" << std::endl; + } + + std::cout << "=== LocalPlannerAdapter destructor finished ===" << std::endl; + } + + /** + * @brief Load the nav_core2 local planner and initialize it + */ + void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) + { + tf_ = createSharedPointerWithNoDelete(tf); + costmap_robot_ = costmap_robot; + costmap_adapter_ = std::make_shared(); + costmap_adapter_->initialize(costmap_robot); + + YAML::Node nh; + private_nh_ = YAML::Node("~"); + adapter_nh_ = YAML::Node("~/" + name); + std::cout << "Adapter namespace: " << adapter_nh_["~"].as() << std::endl; + + std::string planner_name; + if (adapter_nh_["planner_name"] && adapter_nh_["planner_name"].IsDefined()) + { + planner_name = adapter_nh_["planner_name"].as(); + } + else + { + planner_name = nav_2d_utils::loadParameterWithDeprecation( + adapter_nh_, "planner_name", "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); + adapter_nh_["planner_name"] = planner_name; + } + std::cout << "Loading plugin " << planner_name << std::endl; + + // planner_ = planner_loader_.createInstance(planner_name); + std::string path_file_so; + planner_loader_ = boost::dll::import_alias( + path_file_so, planner_name, boost::dll::load_mode::append_decorations); + planner_ = planner_loader_(); + if (!planner_) + { + std::cerr << "Failed to load planner " << planner_name << std::endl; + exit(EXIT_FAILURE); + } + planner_->initialize(private_nh_, adapter_nh_["planner_name"].as(), tf_, costmap_robot_); + + // odom_sub_ = std::make_shared(nh); + // dsrv_ = std::make_shared>(configuration_mutex_, adapter_nh_); + // dynamic_reconfigure::Server::CallbackType cb = + // boost::bind(&LocalPlannerAdapter::reconfigureCB, this, _1, _2); + // dsrv_->setCallback(cb); + } + + bool LocalPlannerAdapter::swapPlanner(std::string planner_name) + { + boost::recursive_mutex::scoped_lock l(configuration_mutex_); + if (!setup_) + { + return false; + } + + if (planner_name != last_config_.planner_name) + { + nav_core2::LocalPlanner::Ptr new_planner; + + try + { + // Tạo planner mới + std::string path_file_so; + planner_loader_ = boost::dll::import_alias( + path_file_so, planner_name, boost::dll::load_mode::append_decorations); + new_planner = planner_loader_(); + if (!new_planner) + { + std::cerr << "Failed to load planner " << planner_name << std::endl; + exit(EXIT_FAILURE); + } + new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as(), + tf_, costmap_robot_); + if (!new_planner) + { + std::cerr << "Failed to load planner " << planner_name << std::endl; + exit(EXIT_FAILURE); + } + new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as(), + tf_, costmap_robot_); + + if (planner_) + planner_.reset(); + planner_ = new_planner; + + last_config_.planner_name = planner_name; + std::cout << "Loaded new planner: " << planner_name << std::endl; + } + catch (const std::exception &ex) + { + std::cerr << "Failed to load planner " << planner_name << ": " << ex.what() << std::endl; + return false; + } + } + return true; + } + + /** + * @brief Collect the additional information needed by nav_core2 and call the child computeVelocityCommands + */ + bool LocalPlannerAdapter::computeVelocityCommands(geometry_msgs::Twist &cmd_vel) + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + if (!has_active_goal_) + { + return false; + } + + // Get the Pose + nav_2d_msgs::Pose2DStamped pose2d; + if (!getRobotPose(pose2d)) + { + return false; + } + + // Get the Velocity + nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist(); + nav_2d_msgs::Twist2DStamped cmd_vel_2d; + try + { + cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity); + } + catch (const nav_core2::PlannerException &e) + { + std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl; + return false; + } + cmd_vel = nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity); + return true; + } + + bool LocalPlannerAdapter::setTwistLinear(geometry_msgs::Vector3 linear) + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + try + { + if (planner_) + return planner_->setTwistLinear(linear); + else + throw std::runtime_error("Failed to set linear"); + } + catch (const std::exception &e) + { + throw std::exception(e); + } + } + + geometry_msgs::Vector3 LocalPlannerAdapter::getTwistLinear(bool direct) + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + try + { + if (planner_) + return planner_->getTwistLinear(direct); + else + throw std::runtime_error("Failed to get linear"); + } + catch (const std::exception &e) + { + throw std::exception(e); + } + } + + bool LocalPlannerAdapter::setTwistAngular(geometry_msgs::Vector3 angular) + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + try + { + if (planner_) + return planner_->setTwistAngular(angular); + else + throw std::runtime_error("Failed to set angular"); + } + catch (const std::exception &e) + { + throw std::exception(e); + } + } + + geometry_msgs::Vector3 LocalPlannerAdapter::getTwistAngular(bool direct) + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + try + { + if (planner_) + return planner_->getTwistAngular(direct); + else + throw std::runtime_error("Failed to get angular"); + } + catch (const std::exception &e) + { + throw std::exception(e); + } + } + + geometry_msgs::Twist LocalPlannerAdapter::getActualTwist() + { + if (odom_sub_) + return nav_2d_utils::twist2Dto3D(odom_sub_->getTwist()); + else + throw std::runtime_error("Failed to get twist"); + } + + bool LocalPlannerAdapter::islock() + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + try + { + if (planner_) + return planner_->islock(); + else + throw std::runtime_error("Failed to call funcion islock"); + } + catch (const std::exception &e) + { + std::cerr << e.what() << std::endl; + throw std::exception(e); + } + } + + void LocalPlannerAdapter::lock() + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + try + { + if (planner_) + planner_->lock(); + else + throw std::runtime_error("Failed to call funcion lock"); + } + catch (const std::exception &e) + { + std::cerr << e.what() << std::endl; + throw std::exception(e); + } + } + + void LocalPlannerAdapter::unlock() + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + try + { + if (planner_) + planner_->unlock(); + else + throw std::runtime_error("Failed to call funcion unlock"); + } + catch (const std::exception &e) + { + std::cerr << e.what() << std::endl; + throw std::exception(e); + } + } + + /** + * @brief Collect the additional information needed by nav_core2 and call the child isGoalReached + */ + bool LocalPlannerAdapter::isGoalReached() + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + // Get the Pose + nav_2d_msgs::Pose2DStamped pose2d; + if (!getRobotPose(pose2d)) + return false; + + nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist(); + bool ret = planner_->isGoalReached(pose2d, velocity); + if (ret) + { + has_active_goal_ = false; + } + return ret; + } + + /** + * @brief Convert from 2d to 3d and call child setPlan + */ + bool LocalPlannerAdapter::setPlan(const std::vector &orig_global_plan) + { + boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); + nav_2d_msgs::Path2D path = nav_2d_utils::posesToPath2D(orig_global_plan); + try + { + if (path.poses.size() > 0) + { + nav_2d_msgs::Pose2DStamped goal_pose; + goal_pose = path.poses.back(); + + if (!has_active_goal_ || hasGoalChanged(goal_pose)) + { + last_goal_ = goal_pose; + has_active_goal_ = true; + planner_->setGoalPose(goal_pose); + planner_->setPlan(path); + } + } + return true; + } + catch (const nav_core2::PlannerException &e) + { + std::cerr << "setPlan Exception: " << e.what() << std::endl; + return false; + } + } + + bool LocalPlannerAdapter::hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal) + { + if (last_goal_.header.frame_id != new_goal.header.frame_id || + last_goal_.header.stamp.toSec() != new_goal.header.stamp.toSec()) + { + return true; + } + + return last_goal_.pose.x != new_goal.pose.x || last_goal_.pose.y != new_goal.pose.y || + last_goal_.pose.theta != new_goal.pose.theta; + } + + bool LocalPlannerAdapter::getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d) + { + geometry_msgs::PoseStamped current_pose; + if (!costmap_robot_->getRobotPose(current_pose)) + { + std::cout << "Could not get robot pose" << std::endl; + return false; + } + pose2d = nav_2d_utils::poseStampedToPose2D(current_pose); + return true; + } + + nav_core::BaseLocalPlanner::Ptr LocalPlannerAdapter::create() + { + return std::make_shared(); + } +} // namespace nav_core_adapter + +// register this planner as a BaseLocalPlanner plugin +BOOST_DLL_ALIAS(nav_core_adapter::LocalPlannerAdapter::create, LocalPlannerAdapter) \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp b/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp new file mode 100755 index 0000000..0271723 --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp @@ -0,0 +1,69 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 + + +TEST(LocalPlannerAdapter, unload_local_planner) +{ + tf2_ros::Buffer tf; + tf.setUsingDedicatedThread(true); + + // This empty transform is added to satisfy the constructor of + // Costmap2DROS, which waits for the transform from map to base_link + // to become available. + geometry_msgs::TransformStamped base_rel_map; + base_rel_map.child_frame_id = "base_link"; + base_rel_map.header.frame_id = "map"; + base_rel_map.transform.rotation.w = 1.0; + tf.setTransform(base_rel_map, "unload", true); + + nav_core_adapter::LocalPlannerAdapter* lpa = new nav_core_adapter::LocalPlannerAdapter(); + + costmap_2d::Costmap2DROS costmap_ros("local_costmap", tf); + lpa->initialize("lpa", &tf, &costmap_ros); + + delete lpa; + + // Simple test to make sure costmap hasn't been deleted + EXPECT_EQ("map", costmap_ros.getGlobalFrameID()); +} + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "unload_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/Navigations/Cores/nav_core_adapter/test/unload_test.launch b/src/Navigations/Cores/nav_core_adapter/test/unload_test.launch new file mode 100755 index 0000000..c1ddad8 --- /dev/null +++ b/src/Navigations/Cores/nav_core_adapter/test/unload_test.launch @@ -0,0 +1,3 @@ + + + diff --git a/src/Navigations/Libraries/nav_grid/CMakeLists.txt b/src/Navigations/Libraries/nav_grid/CMakeLists.txt new file mode 100755 index 0000000..8b1058c --- /dev/null +++ b/src/Navigations/Libraries/nav_grid/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.10) + +project(nav_grid VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# Tìm tất cả header files +file(GLOB_RECURSE HEADERS "include/nav_grid/*.h") + +# Tạo INTERFACE library (header-only) +add_library(nav_grid INTERFACE) + +# Set include directories +target_include_directories(nav_grid + INTERFACE + $ + $ +) + +# Cài đặt header files +install(DIRECTORY include/nav_grid + DESTINATION include + FILES_MATCHING PATTERN "*.h") + +# Cài đặt target +install(TARGETS nav_grid + EXPORT nav_grid-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# Export targets +install(EXPORT nav_grid-targets + FILE nav_grid-targets.cmake + NAMESPACE nav_grid:: + DESTINATION lib/cmake/nav_grid) + +# Tùy chọn build tests +option(BUILD_TESTING "Build test programs" OFF) + +if(BUILD_TESTING) + find_package(GTest REQUIRED) + + # Tạo test executable + add_executable(nav_grid_test test/utest.cpp) + target_link_libraries(nav_grid_test + PRIVATE + nav_grid + GTest::GTest + GTest::Main + ) + + # Thêm test vào CTest + enable_testing() + add_test(NAME nav_grid_test COMMAND nav_grid_test) +endif() + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME} (Header-Only)") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Headers found:") +foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") +endforeach() +message(STATUS "Build tests: ${BUILD_TESTING}") +message(STATUS "=================================") diff --git a/src/Navigations/Libraries/nav_grid/README.md b/src/Navigations/Libraries/nav_grid/README.md new file mode 100755 index 0000000..7ec1957 --- /dev/null +++ b/src/Navigations/Libraries/nav_grid/README.md @@ -0,0 +1,39 @@ +# nav_grid + +Many navigation algorithms rely on the concept of a two dimensional grid being overlaid on the world, with a value being assigned to each grid cell. In the original navigation stack, `Costmap2DROS` associated an `unsigned char` with grid cells, global planners cache distance to the goal as `float`s and local planners would cache various metrics in a grid to quickly calculate the strength of different trajectories. + +![nav_grid diagram](doc/nav_grid.png) + +## `NavGridInfo` + +Where the grid exists in the world is defined by six parameters. + * `width` and `height` (which together define the number of cells in the grid) + * `resolution` which is the dimension of each cell in meters (square cells only) + * `frame_id` which is the TF frame the grid is defined relative to. + * `origin_x` and `origin_y` which define the offset in meters from the root of the TF frame to the minimum corner of the (0, 0) cell. + +Together, these components make a [`nav_grid::NavGridInfo`](include/nav_grid/nav_grid_info.h) struct. It evolved from the [`nav_msgs::MapMetaData` message](http://docs.ros.org/melodic/api/nav_msgs/html/msg/MapMetaData.html) but without `map_load_time`, simplified origin geometry and the `frame_id` added. Note: for general efficiency of computation (particularly moving the grid while retaining some of the values) there is no rotation component to the origin. Each grid is locked to the orientation of its TF frame. + +The default values are `width=0, height=0, resolution=1.0, frame_id="map", origin_x=0.0, origin_y=0.0`. + +## Coordinate Conversion +One of the most common operations is to want to convert between the real world coordinates and the grid coordinates. These operations can be done with a `NavGridInfo` object and the methods in [`coordinate_conversion.h`](include/nav_grid/coordinate_conversion.h). They are derived from methods in [`costmap_2d.h`](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/costmap_2d.h#L126), but with some key differences (beyond replacing `map` with `grid`). + * `gridToWorld` is the same as `mapToWorld`, as both return the world coordinates of the center of the specified cell. + * `worldToGrid` works like `worldToMapNoBounds`, but it results in either `int` or `double` coordinates depending on the output parameter types. As the result are not bounded by the grid, the results are signed. + * `worldToGridBounded` is a combination of `worldToMap` and `worldToMapEnforceBounds`. It returns a bool for whether the input coordinates are within the grid AND the output coordinates are forced to be within the grid. The output coordinates are therefore `unsigned int`. + * There's also `isWithinGrid` that returns whether a given point is within the grid (i.e. will match the return value of `worldToGridBounded` but saves some of the computation associated with calculating the actual values of the coordinates. + +![example coordinate conversion](doc/coords.png) + +## `NavGrid` +Of course, we also want to associate a value with each cell in the grid. For that, we define the templatized [`nav_grid::NavGrid`](include/nav_grid/nav_grid.h) abstract class. The template allows for storing arbitrary data types associated with each grid cell. The actual storage mechanism for the data is not part of the base class to allow for possibly more efficient methods. A default implementation where the data is simply stored in row-major order in a one-dimensional vector is provided in [`nav_grid::VectorNavGrid`](include/nav_grid/vector_nav_grid.h>) + +The constructor for `NavGrid` takes a default value for each cell which is 0 by default. The grid's initial info matches the default info above, so the grid is initially `0x0`. + +The `NavGrid` class provides handy methods for accessing values via their grid indexes. You can use `grid(x, y)` or `grid.getValue(x, y)` to access each value, and use `grid.setValue(x, y, value)` to write each value. There is also the helper class [`nav_grid::Index`](include/nav_grid/index.h) that can be used to store the two coordinates and used in accessing the data as well a la `grid(index)` and `grid.setValue(index, value)`. + +There are two methods for changing the `info` associated with the grid: `setInfo` and `updateInfo`. `setInfo` changes the `info` while maintaining the data associated with each grid coordinate. `updateInfo` will change the info but instead maintain the data associated with the world coordinates. + +For instance, imagine a 5x5 grid with 0.5 meter resolution with the cell (2, 0) set to red which represents a cell at (1.25, 0.25) in the world. If we change the origin to be 0.5 meters to the right, the grids will have different values according to the method we use. With `setInfo`, cell (2, 0) is still red, but it is associated with a cell at (1.75, 0.25) in the world. With `updateInfo`, the cell at (1.25, 0.25) is still red, but it is now associated with cell (1, 0). The exact mechanism for how this data is preserved is left to the implementing class. + +![illustration of grid update](doc/change_info.png) diff --git a/src/Navigations/Libraries/nav_grid/doc/change_info.png b/src/Navigations/Libraries/nav_grid/doc/change_info.png new file mode 100755 index 0000000..1e0e9c5 Binary files /dev/null and b/src/Navigations/Libraries/nav_grid/doc/change_info.png differ diff --git a/src/Navigations/Libraries/nav_grid/doc/coords.png b/src/Navigations/Libraries/nav_grid/doc/coords.png new file mode 100755 index 0000000..dfa40bf Binary files /dev/null and b/src/Navigations/Libraries/nav_grid/doc/coords.png differ diff --git a/src/Navigations/Libraries/nav_grid/doc/nav_grid.png b/src/Navigations/Libraries/nav_grid/doc/nav_grid.png new file mode 100755 index 0000000..c43cda0 Binary files /dev/null and b/src/Navigations/Libraries/nav_grid/doc/nav_grid.png differ diff --git a/src/Navigations/Libraries/nav_grid/include/nav_grid/coordinate_conversion.h b/src/Navigations/Libraries/nav_grid/include/nav_grid/coordinate_conversion.h new file mode 100755 index 0000000..4eb06fe --- /dev/null +++ b/src/Navigations/Libraries/nav_grid/include/nav_grid/coordinate_conversion.h @@ -0,0 +1,168 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_GRID_COORDINATE_CONVERSION_H +#define NAV_GRID_COORDINATE_CONVERSION_H + +#include +#include + +namespace nav_grid +{ +/** + * @brief Convert from grid coordinates to world coordinates of the center of the cell + * + * The resulting coordinates are for the center of the grid cell. + * + * @param[in] mx The x grid coordinate + * @param[in] my The y grid coordinate + * @param[out] wx Set to the associated x world coordinate + * @param[out] wy Set to the associated y world coordinate + */ +inline void gridToWorld(const NavGridInfo& info, int mx, int my, double& wx, double& wy) +{ + wx = info.origin_x + (mx + 0.5) * info.resolution; + wy = info.origin_y + (my + 0.5) * info.resolution; +} + +/** + * @brief Convert from world coordinates to the precise (double) grid coordinates + * + * The results are not rounded, so that the values can be used for locating a position within a cell + * + * @param[in] wx The x world coordinate + * @param[in] wy The y world coordinate + * @param[out] mx Set to the associated x grid coordinate + * @param[out] my Set to the associated y grid coordinate + */ +inline void worldToGrid(const NavGridInfo& info, double wx, double wy, double& mx, double& my) +{ + mx = (wx - info.origin_x) / info.resolution; + my = (wy - info.origin_y) / info.resolution; +} + +/** + * @brief Convert from world coordinates to grid coordinates without checking for legal bounds + * @param[in] wx The x world coordinate + * @param[in] wy The y world coordinate + * @param[out] mx Set to the associated x grid coordinate + * @param[out] my Set to the associated y grid coordinate + * @note The returned grid coordinates are not guaranteed to lie within the grid. + */ +inline void worldToGrid(const NavGridInfo& info, double wx, double wy, int& mx, int& my) +{ + double dmx, dmy; + worldToGrid(info, wx, wy, dmx, dmy); + mx = static_cast(floor(dmx)); + my = static_cast(floor(dmy)); +} + +/** + * @brief Convert from world coordinates to grid coordinates + * + * Combined functionality from costmap_2d::worldToMap and costmap_2d::worldToMapEnforceBounds. + * The output parameters are set to grid indexes within the grid, even if the function returns false, + * meaning the coordinates are outside the grid. + * + * @param[in] wx The x world coordinate + * @param[in] wy The y world coordinate + * @param[out] mx Set to the associated (bounds-enforced) x grid coordinate + * @param[out] my Set to the associated (bounds-enforced) y grid coordinate + * @return True if the input coordinates were within the grid + */ +inline bool worldToGridBounded(const NavGridInfo& info, double wx, double wy, unsigned int& mx, unsigned int& my) +{ + double dmx, dmy; + worldToGrid(info, wx, wy, dmx, dmy); + + bool valid = true; + + if (dmx < 0.0) + { + mx = 0; + valid = false; + } + else if (dmx >= info.width) + { + mx = info.width - 1; + valid = false; + } + else + { + mx = static_cast(dmx); + } + + if (dmy < 0.0) + { + my = 0; + valid = false; + } + else if (dmy >= info.height) + { + my = info.height - 1; + valid = false; + } + else + { + my = static_cast(dmy); + } + + return valid; +} + +/** + * @brief Check to see if the world coordinates are within the grid. + * + * This should only be used if the caller does not need the associated grid coordinates. Otherwise it would + * be more efficient to call worldToGridBounded. + * + * @param[in] wx The x world coordinate + * @param[in] wy The y world coordinate + * @return True if the input coordinates were within the grid + */ +inline bool isWithinGrid(const NavGridInfo& info, double wx, double wy) +{ + wx -= info.origin_x; + wy -= info.origin_y; + return wx >= 0.0 && + wy >= 0.0 && + wx < info.width * info.resolution && + wy < info.height * info.resolution; +} + + + +} // namespace nav_grid + +#endif // NAV_GRID_COORDINATE_CONVERSION_H diff --git a/src/Navigations/Libraries/nav_grid/include/nav_grid/index.h b/src/Navigations/Libraries/nav_grid/include/nav_grid/index.h new file mode 100755 index 0000000..e6be03d --- /dev/null +++ b/src/Navigations/Libraries/nav_grid/include/nav_grid/index.h @@ -0,0 +1,99 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_GRID_INDEX_H +#define NAV_GRID_INDEX_H + +#include + +namespace nav_grid +{ +/** + * @class GenericIndex + * @brief A simple pair of x/y coordinates + */ +template +struct GenericIndex +{ + NumericType x, y; + explicit GenericIndex(const NumericType& x = 0, const NumericType& y = 0) : x(x), y(y) {} + + /** + * @brief comparison operator that requires equal x and y + */ + bool operator == (const GenericIndex& other) const + { + return x == other.x && y == other.y; + } + + bool operator != (const GenericIndex& other) const + { + return !operator==(other); + } + + /** + * @brief less than operator so object can be used in sets + */ + bool operator < (const GenericIndex& other) const + { + return x < other.x || (x == other.x && y < other.y); + } + + // Derived Comparators + bool operator > (const GenericIndex& other) const { return other < *this; } + bool operator <= (const GenericIndex& other) const { return !(*this > other); } + bool operator >= (const GenericIndex& other) const { return !(*this < other); } + + /** + * @brief String representation of this object + */ + std::string toString() const + { + return "(" + std::to_string(x) + ", " + std::to_string(y) + ")"; + } +}; + +template +inline std::ostream& operator<<(std::ostream& stream, const GenericIndex& index) +{ + stream << index.toString(); + return stream; +} + +using SignedIndex = GenericIndex; +using Index = GenericIndex; + +} // namespace nav_grid + +#endif // NAV_GRID_INDEX_H diff --git a/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid.h b/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid.h new file mode 100755 index 0000000..7dfe7f8 --- /dev/null +++ b/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid.h @@ -0,0 +1,157 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_GRID_NAV_GRID_H +#define NAV_GRID_NAV_GRID_H + +#include +#include +#include + +namespace nav_grid +{ +/** + * @class NavGrid + * This class is a spiritual successor to the costmap_2d::Costmap2D class, with the key differences being that + * the datatype and data storage methods are not specified, and the frame_id is specified. + * + * The templatized nature of the class allows you to store whatever you like at each grid location, including + * unsigned chars if emulating Costmap2D or floating point numbers if emulating the grid_map package, or whatever + * else. + * + * The VectorNavGrid class in this package implements this class with a straight-forward single-dimensional vector + * representing the two dimensional grid. Other classes could implement the data storage differently. + * + * Getting data from the grid can be done either through the getValue methods or the parenthetical operators (which call + * getValue internally). Implementing classes must implement getValue. + * x = grid(0, 0) + grid.getValue(0, 1); + * + * Writing data to the grid must be done through the setValue method (which implementing classes must implement) + * grid.setValue(0, 0, x); + * + * You can also use nav_grid::Index objects + * nav_grid::Index index(0, 0); + * x = grid(index) + grid.getValue(index); + * index.y = 3; + * grid.setCost(index, x); + * The Index methods also internally call setValue/getValue + * + * The geometry of the grid is specified by the NavGridInfo. Borrowing an idea from the grid_map package, two + * separate methods are defined for changing the info. setInfo will change the info without changing the grid values. + * updateInfo will change the info while trying to preserve the contents of the grid. + * + * The final component is a collection of methods inspired by Costmap2D for converting coordinates of different types. + */ +template class NavGrid +{ +public: + explicit NavGrid(const T default_value = T{}) : default_value_(default_value) {} + + /** + * @brief Reset the contents of the grid + */ + virtual void reset() = 0; + + /** + * @brief get the value of the grid at (x,y) + * @param x[in] Valid x coordinate + * @param y[in] Valid y coordinate + * @return value at (x,y) + */ + virtual T getValue(const unsigned int x, const unsigned int y) const = 0; + + /** + * @brief set the value of the grid at (x,y) + * @param x[in] Valid x coordinate + * @param y[in] Valid y coordinate + * @param value[in] New Value + */ + virtual void setValue(const unsigned int x, const unsigned int y, const T& value) = 0; + + /**@name Convenience Aliases */ + // Note: You may not be able to use these unless your deriving class declares using NavGrid::operator() or + // using NavGrid::getValue + /**@{*/ + T getValue(const Index& index) { return getValue(index.x, index.y); } + T operator() (const unsigned int x, const unsigned int y) const { return getValue(x, y); } + T operator() (const Index& index) const { return getValue(index.x, index.y); } + void setValue(const Index& index, const T& value) { setValue(index.x, index.y, value); } + + /**@}*/ + + /** + * @brief Change the info while attempting to keep the values associated with the grid coordinates + * @param[in] new_info New grid info + */ + virtual void setInfo(const NavGridInfo& new_info) = 0; + + /** + * @brief Change the info while attempting to keep the values associated with the world coordinates + * + * For example, if the only change to the info is to the origin's x coordinate (increasing by an amount equal to the + * resolution), then all the values should be shifted one grid cell to the left. + * + * @param[in] new_info New grid info + */ + virtual void updateInfo(const NavGridInfo& new_info) { setInfo(new_info); } + + inline NavGridInfo getInfo() const { return info_; } + + /** + * @brief Set the default value + * @param[in] new_value New Default Value + */ + void setDefaultValue(const T new_value) + { + default_value_ = new_value; + } + + /***************************************************************************************************** + * NavGridInfo accessor methods + *****************************************************************************************************/ + inline unsigned int getWidth() const { return info_.width; } + inline unsigned int getHeight() const { return info_.height; } + inline double getResolution() const { return info_.resolution; } + inline std::string getFrameId() const { return info_.frame_id; } + inline double getOriginX() const { return info_.origin_x; } + inline double getOriginY() const { return info_.origin_y; } + +protected: + NavGridInfo info_; + T default_value_; +}; + +} // namespace nav_grid + +#endif // NAV_GRID_NAV_GRID_H diff --git a/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid_info.h b/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid_info.h new file mode 100755 index 0000000..8addf08 --- /dev/null +++ b/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid_info.h @@ -0,0 +1,92 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_GRID_NAV_GRID_INFO_H +#define NAV_GRID_NAV_GRID_INFO_H + +#include + +namespace nav_grid +{ +/** + * @struct NavGridInfo + * This class defines a way to discretize a finite section of the world into a grid. + * It contains similar information to the ROS msg nav_msgs/MapMetaData (aka the info field of nav_msgs/OccupancyGrid) + * except the map_load_time is removed, the geometry is simplified from a Pose to xy coordinates, and the frame_id + * is added. + */ +struct NavGridInfo +{ +public: + /* All data is publically accessible */ + unsigned int width = 0; + unsigned int height = 0; + double resolution = 1.0; + std::string frame_id = "map"; + double origin_x = 0.0; ///< The origin defines the coordinates of minimum corner of cell (0,0) in the grid + double origin_y = 0.0; + + /** + * @brief comparison operator that requires all fields are equal + */ + bool operator == (const NavGridInfo& info) const + { + return width == info.width && height == info.height && resolution == info.resolution && + origin_x == info.origin_x && origin_y == info.origin_y && frame_id == info.frame_id; + } + + bool operator != (const NavGridInfo& info) const + { + return !operator==(info); + } + + /** + * @brief String representation of this object + */ + std::string toString() const + { + return std::to_string(width) + "x" + std::to_string(height) + " (" + std::to_string(resolution) + "res) " + + frame_id + " " + std::to_string(origin_x) + " " + std::to_string(origin_y); + } +}; + +inline std::ostream& operator<<(std::ostream& stream, const NavGridInfo& info) +{ + stream << info.toString(); + return stream; +} + +} // namespace nav_grid + +#endif // NAV_GRID_NAV_GRID_INFO_H diff --git a/src/Navigations/Libraries/nav_grid/include/nav_grid/vector_nav_grid.h b/src/Navigations/Libraries/nav_grid/include/nav_grid/vector_nav_grid.h new file mode 100755 index 0000000..ea7c4a9 --- /dev/null +++ b/src/Navigations/Libraries/nav_grid/include/nav_grid/vector_nav_grid.h @@ -0,0 +1,239 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 NAV_GRID_VECTOR_NAV_GRID_H +#define NAV_GRID_VECTOR_NAV_GRID_H + +#include +#include +#include +#include + +namespace nav_grid +{ +/** + * @class VectorNavGrid + * A straight-forward implementation of the NavGrid class where the data for cell (x, y) is stored in a std::vector + * with index (y * info.width + x). + */ +template class VectorNavGrid : public NavGrid +{ +public: + using NavGrid::NavGrid; + + /** + * @brief Reset the contents of the grid to the default value + */ + void reset() override + { + data_.assign(this->info_.width * this->info_.height, this->default_value_); + } + + /** + * @brief Change the info while attempting to keep the values associated with the grid coordinates + * + * If the width changes, we need to move each row to its new location + * + * If just the height changes, then we can resize the vector without having to move elements + * + * We just overwrite the rest of the grid info + */ + void setInfo(const NavGridInfo& new_info) override + { + if (this->info_.width != new_info.width) + { + std::vector new_vector(new_info.width * new_info.height, this->default_value_); + unsigned int cols_to_move = std::min(this->info_.width, new_info.width); + auto old_it = data_.begin(); + auto new_it = new_vector.begin(); + unsigned int max_row = std::min(this->info_.height, new_info.height); + for (unsigned int row = 0; row < max_row; row++) + { + std::copy(old_it, old_it + cols_to_move, new_it); + old_it += this->info_.width; + new_it += new_info.width; + } + data_.swap(new_vector); + } + else if (this->info_.height != new_info.height) + { + data_.resize(new_info.width * new_info.height, this->default_value_); + } + + this->info_ = new_info; + } + + /** + * @brief Update the info while keeping the data geometrically in tact + * + * If the resolution or frame_id changes, reset all the data. + * + * Otherwise, adjust the new_info so the grid stays aligned (The grid's new info will be within a + * resolution-length of the original new_info). Then copy the common values into the new grid. + * + * @param[in] new_info New information to update the grid with + */ + void updateInfo(const NavGridInfo& new_info) override + { + // If the info is the same, make no changes + if (this->info_ == new_info) + { + return; + } + + // If the resolution or frame changes, reset the whole grid + if (this->info_.resolution != new_info.resolution || this->info_.frame_id != new_info.frame_id) + { + setInfo(new_info); + return; + } + + // project the new origin into the grid + int cell_ox, cell_oy; + worldToGrid(this->info_, new_info.origin_x, new_info.origin_y, cell_ox, cell_oy); + + // To save casting from unsigned int to int a bunch of times + int old_size_x = static_cast(this->info_.width); + int old_size_y = static_cast(this->info_.height); + + // we need to compute the overlap of the new and existing windows + int lower_left_x = std::min(std::max(cell_ox, 0), old_size_x); + int lower_left_y = std::min(std::max(cell_oy, 0), old_size_y); + int upper_right_x = std::min(std::max(cell_ox + static_cast(new_info.width), 0), old_size_x); + int upper_right_y = std::min(std::max(cell_oy + static_cast(new_info.height), 0), old_size_y); + + unsigned int cell_size_x = upper_right_x - lower_left_x; + unsigned int cell_size_y = upper_right_y - lower_left_y; + + // we need a vector to store the new contents in the window temporarily + std::vector new_data(new_info.width * new_info.height, this->default_value_); + + // compute the starting cell location for copying data back in + int start_x = lower_left_x - cell_ox; + int start_y = lower_left_y - cell_oy; + + // now we want to copy the overlapping information into the new vector, but in its new location + // we'll first need to compute the starting points for each vector + auto src_index = data_.begin() + (lower_left_y * old_size_x + lower_left_x); + auto dest_index = new_data.begin() + (start_y * new_info.width + start_x); + + // now, we'll copy the source vector into the destination vector + for (unsigned int i = 0; i < cell_size_y; ++i) + { + std::copy(src_index, src_index + cell_size_x, dest_index); + src_index += this->info_.width; + dest_index += new_info.width; + } + + data_.swap(new_data); + + // update the dimensions + this->info_.width = new_info.width; + this->info_.height = new_info.height; + + // update the origin. Recomputed instead of using new_info.origin + // because we want to keep things grid-aligned + this->info_.origin_x += cell_ox * this->info_.resolution; + this->info_.origin_y += cell_oy * this->info_.resolution; + } + + void setValue(const unsigned int x, const unsigned int y, const T& value) override + { + data_[getIndex(x, y)] = value; + } + + T getValue(const unsigned int x, const unsigned int y) const override + { + return data_[getIndex(x, y)]; + } + + using NavGrid::operator(); + using NavGrid::getValue; + using NavGrid::setValue; + + /** + * Overloading the [] operator so that the data can be accessed directly with vector_nav_grid[i] + */ + T operator[] (unsigned int i) const {return data_[i];} + T& operator[] (unsigned int i) {return data_[i];} + + /** + * @brief Return the size of the vector. Equivalent to width * height. + * @return size of the vector + */ + unsigned int size() const { return data_.size(); } + + /** + * @brief Given two grid coordinates... compute the associated index + * @param[in] mx The x coordinate + * @param[in] my The y coordinate + * @return The associated index + */ + inline unsigned int getIndex(unsigned int mx, unsigned int my) const + { + return my * this->info_.width + mx; + } + + /** + * @brief Given two world coordinates... compute the associated index + * @param[in] mx The x coordinate + * @param[in] my The y coordinate + * @return The associated index + */ + inline unsigned int getIndex(double x, double y) const + { + unsigned int mx, my; + worldToGridBounded(this->info_, x, y, mx, my); + return getIndex(mx, my); + } + + /** + * @brief Given an index... compute the associated grid coordinates + * @param[in] index The index + * @param[out] mx Set to the associated x grid coordinate + * @param[out] my Set to the associated y grid coordinate + */ + inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const + { + unsigned int w = this->info_.width; + my = index / w; + mx = index - my * w; + } + +protected: + std::vector data_; +}; +} // namespace nav_grid + +#endif // NAV_GRID_VECTOR_NAV_GRID_H diff --git a/src/Navigations/Libraries/nav_grid/test/utest.cpp b/src/Navigations/Libraries/nav_grid/test/utest.cpp new file mode 100755 index 0000000..c9463cf --- /dev/null +++ b/src/Navigations/Libraries/nav_grid/test/utest.cpp @@ -0,0 +1,522 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * 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 + +TEST(VectorNavGrid, info_equality) +{ + nav_grid::NavGridInfo info0; + nav_grid::NavGridInfo info1; + nav_grid::NavGridInfo width_info; + width_info.width = 3; + + nav_grid::NavGridInfo height_info; + height_info.height = 3; + + nav_grid::NavGridInfo res_info; + res_info.resolution = 3.0; + + nav_grid::NavGridInfo frame_info; + frame_info.frame_id = "foobar"; + + nav_grid::NavGridInfo originx_info; + originx_info.origin_x = 3.0; + + nav_grid::NavGridInfo originy_info; + originy_info.origin_y = 3.0; + + + EXPECT_EQ(info0, info0); + EXPECT_EQ(info0, info1); + EXPECT_NE(info0, width_info); + EXPECT_NE(info0, height_info); + EXPECT_NE(info0, res_info); + EXPECT_NE(info0, frame_info); + EXPECT_NE(info0, originx_info); + EXPECT_NE(info0, originy_info); +} + +TEST(VectorNavGrid, basic_test) +{ + nav_grid::VectorNavGrid grid(-3); + nav_grid::NavGridInfo info; + info.width = 2; + info.height = 3; + grid.setInfo(info); + EXPECT_EQ(grid(0, 0), -3); + grid.setValue(1, 1, 10); + EXPECT_EQ(grid(0, 0), -3); + EXPECT_EQ(grid(1, 1), 10); +} + +TEST(VectorNavGrid, basic_index_test) +{ + nav_grid::VectorNavGrid grid(-3); + nav_grid::NavGridInfo info; + info.width = 2; + info.height = 3; + grid.setInfo(info); + + nav_grid::Index index0(0, 0), index1(1, 1); + EXPECT_EQ(grid(index0), -3); + grid.setValue(index1, 10); + EXPECT_EQ(grid(index0), -3); + EXPECT_EQ(grid(index1), 10); + EXPECT_EQ(grid(0, 0), -3); + EXPECT_EQ(grid(1, 1), 10); +} + +TEST(VectorNavGrid, easy_coordinates_test) +{ + nav_grid::NavGridInfo info; + info.width = 2; + info.height = 3; + + double wx, wy; + gridToWorld(info, 0, 0, wx, wy); + EXPECT_DOUBLE_EQ(wx, 0.5); + EXPECT_DOUBLE_EQ(wy, 0.5); + gridToWorld(info, 1, 2, wx, wy); + EXPECT_DOUBLE_EQ(wx, 1.5); + EXPECT_DOUBLE_EQ(wy, 2.5); + + unsigned int umx, umy; + int mx, my; + double dmx, dmy; + ASSERT_TRUE(worldToGridBounded(info, wx, wy, umx, umy)); + EXPECT_EQ(umx, 1); + EXPECT_EQ(umy, 2); + worldToGrid(info, wx, wy, mx, my); + EXPECT_EQ(mx, 1); + EXPECT_EQ(my, 2); + worldToGrid(info, wx, wy, dmx, dmy); + EXPECT_DOUBLE_EQ(dmx, wx); + EXPECT_DOUBLE_EQ(dmy, wy); + + // Invalid Coordinate + wx = 2.5; + EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy)); + EXPECT_EQ(umx, 1); + EXPECT_EQ(umy, 2); + worldToGrid(info, wx, wy, mx, my); + EXPECT_EQ(mx, 2); + EXPECT_EQ(my, 2); + + // Border Cases + EXPECT_TRUE(worldToGridBounded(info, 0.0, wy, umx, umy)); + EXPECT_EQ(umx, 0); + EXPECT_TRUE(worldToGridBounded(info, 0.25, wy, umx, umy)); + EXPECT_EQ(umx, 0); + EXPECT_TRUE(worldToGridBounded(info, 0.75, wy, umx, umy)); + EXPECT_EQ(umx, 0); + EXPECT_TRUE(worldToGridBounded(info, 0.9999, wy, umx, umy)); + EXPECT_EQ(umx, 0); + EXPECT_TRUE(worldToGridBounded(info, 1.0, wy, umx, umy)); + EXPECT_EQ(umx, 1); + EXPECT_TRUE(worldToGridBounded(info, 1.25, wy, umx, umy)); + EXPECT_EQ(umx, 1); + EXPECT_TRUE(worldToGridBounded(info, 1.75, wy, umx, umy)); + EXPECT_EQ(umx, 1); + EXPECT_TRUE(worldToGridBounded(info, 1.9999, wy, umx, umy)); + EXPECT_EQ(umx, 1); + EXPECT_FALSE(worldToGridBounded(info, 2.0, wy, umx, umy)); + EXPECT_EQ(umx, 1); +} + +TEST(VectorNavGrid, hard_coordinates_test) +{ + nav_grid::NavGridInfo info; + info.width = 2; + info.height = 3; + info.resolution = 0.1; + info.origin_x = -0.2; + info.origin_y = 0.2; + + double wx, wy; + gridToWorld(info, 0, 0, wx, wy); + EXPECT_DOUBLE_EQ(wx, -0.15); + EXPECT_DOUBLE_EQ(wy, 0.25); + gridToWorld(info, 1, 2, wx, wy); + EXPECT_DOUBLE_EQ(wx, -0.05); + EXPECT_DOUBLE_EQ(wy, 0.45); + + unsigned int umx, umy; + int mx, my; + double dmx, dmy; + EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy)); + EXPECT_EQ(umx, 1); + EXPECT_EQ(umy, 2); + worldToGrid(info, wx, wy, mx, my); + EXPECT_EQ(mx, 1); + EXPECT_EQ(my, 2); + worldToGrid(info, wx, wy, dmx, dmy); + EXPECT_DOUBLE_EQ(dmx, 1.5); + EXPECT_DOUBLE_EQ(dmy, 2.5); + + // Invalid Coordinate + wx = 2.5; + EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy)); + EXPECT_EQ(umx, 1); + EXPECT_EQ(umy, 2); + worldToGrid(info, wx, wy, mx, my); + EXPECT_EQ(mx, 27); + EXPECT_EQ(my, 2); +} + + +TEST(VectorNavGrid, speed_test) +{ + nav_grid::NavGridInfo info; + + const int N = 1000; + const int EXTRA = 300; + + info.width = N; + info.height = N; + + double wx, wy; + unsigned int umx, umy; + int mx, my; + double dmx, dmy; + + for (int x = -EXTRA; x < N + EXTRA; x++) + { + for (int y = -EXTRA; y < N + EXTRA; y++) + { + gridToWorld(info, x, y, wx, wy); + if (x < 0 || y < 0 || x >= N || y >= N) + { + EXPECT_FALSE(isWithinGrid(info, wx, wy)); + EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy)); + EXPECT_EQ(umx, std::min(std::max(0, x), N - 1)); + EXPECT_EQ(umy, std::min(std::max(0, y), N - 1)); + } + else + { + EXPECT_TRUE(isWithinGrid(info, wx, wy)); + EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy)); + EXPECT_EQ(umx, x); + EXPECT_EQ(umy, y); + } + worldToGrid(info, wx, wy, mx, my); + EXPECT_EQ(mx, x); + EXPECT_EQ(my, y); + worldToGrid(info, wx, wy, dmx, dmy); + EXPECT_DOUBLE_EQ(dmx, x + 0.5); + EXPECT_DOUBLE_EQ(dmy, y + 0.5); + } + } +} + +int testGridValue(double x, double y) +{ + return static_cast(100 * floor(x) + floor(y)); +} + +/** + * Initialize Grid Values with values based on the grid/world coordinates + * which are initially the same + * + * x --> + * 000 100 200 300 400 500 600 700 800 900 y + * 001 101 201 301 401 501 601 701 801 901 | + * 002 102 202 302 402 502 602 702 802 902 | + * 003 103 203 303 403 503 603 703 803 903 V + * 004 104 204 304 404 504 604 704 804 904 + + */ +void initializeTestGrid(nav_grid::VectorNavGrid& grid) +{ + grid.setDefaultValue(-10); + nav_grid::NavGridInfo info; + info.width = 10; + info.height = 5; + grid.setInfo(info); + double mx, my; + for (unsigned int j = 0; j < info.height; j++) + { + for (unsigned int i = 0; i < info.width; i++) + { + gridToWorld(info, i, j, mx, my); + grid.setValue(i, j, testGridValue(mx, my)); + } + } +} + +/** + * Check to make sure all the grid values are now the same based on their grid coordinates + */ +void checkSetGridValues(const nav_grid::VectorNavGrid& grid, + unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1) +{ + for (unsigned int x = 0; x < grid.getWidth(); x++) + { + for (unsigned int y = 0; y < grid.getHeight(); y++) + { + if (x >= x0 && x < x1 && y >= y0 && y < y1) + { + EXPECT_EQ(grid(x, y), testGridValue(x, y)); // testGridValue based on Grid Coordinates + } + else + { + EXPECT_EQ(grid(x, y), -10); + } + } + } +} + +/** + * Check to make sure all the grid values are now the same based on their world coordinates + */ +void checkUpdateGridValues(const nav_grid::VectorNavGrid& grid, + unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1) +{ + double mx, my; + for (unsigned int x = 0; x < grid.getWidth(); x++) + { + for (unsigned int y = 0; y < grid.getHeight(); y++) + { + if (x >= x0 && x < x1 && y >= y0 && y < y1) + { + gridToWorld(grid.getInfo(), x, y, mx, my); + EXPECT_EQ(grid(x, y), testGridValue(mx, my)); // testGridValue based on World Coordinates + } + else + { + EXPECT_EQ(grid(x, y), -10); + } + } + } +} + +void debugGridValues(const nav_grid::VectorNavGrid& grid) +{ + for (unsigned int j = 0; j < grid.getHeight(); j++) + { + for (unsigned int i = 0; i < grid.getWidth(); i++) + { + printf("%d ", grid(i, j)); + } + printf("\n"); + } + printf("\n"); +} + +TEST(VectorNavGrid, resizing_grid_with_set) +{ + nav_grid::VectorNavGrid grid; + initializeTestGrid(grid); + checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight()); + + nav_grid::NavGridInfo decreased_width_info = grid.getInfo(); + decreased_width_info.width = 5; + grid.setInfo(decreased_width_info); + checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight()); + + nav_grid::NavGridInfo increased_width_info = grid.getInfo(); + increased_width_info.width = 9; + grid.setInfo(increased_width_info); + checkSetGridValues(grid, 0, 5, 0, grid.getHeight()); + + initializeTestGrid(grid); + checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight()); + + nav_grid::NavGridInfo increased_height_info = grid.getInfo(); + increased_height_info.height = 9; + grid.setInfo(increased_height_info); + checkSetGridValues(grid, 0, grid.getWidth(), 0, 5); + + nav_grid::NavGridInfo decreased_height_info = grid.getInfo(); + decreased_height_info.height = 4; + grid.setInfo(decreased_height_info); + checkSetGridValues(grid, 0, grid.getWidth(), 0, 4); +} + +TEST(VectorNavGrid, resizing_grid_with_update) +{ + nav_grid::VectorNavGrid grid; + initializeTestGrid(grid); + checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight()); + + nav_grid::NavGridInfo decreased_width_info = grid.getInfo(); + decreased_width_info.width = 5; + grid.updateInfo(decreased_width_info); + checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight()); + + nav_grid::NavGridInfo increased_width_info = grid.getInfo(); + increased_width_info.width = 9; + grid.updateInfo(increased_width_info); + checkUpdateGridValues(grid, 0, 5, 0, grid.getHeight()); + + initializeTestGrid(grid); + checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight()); + + nav_grid::NavGridInfo increased_height_info = grid.getInfo(); + increased_height_info.height = 9; + grid.updateInfo(increased_height_info); + checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 5); + + nav_grid::NavGridInfo decreased_height_info = grid.getInfo(); + decreased_height_info.height = 4; + grid.updateInfo(decreased_height_info); + checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 4); +} + +TEST(VectorNavGrid, change_origin) +{ + nav_grid::VectorNavGrid grid; + initializeTestGrid(grid); + + nav_grid::NavGridInfo bump_right_info = grid.getInfo(); + bump_right_info.origin_x = 3; + grid.updateInfo(bump_right_info); + checkUpdateGridValues(grid, 0, 7, 0, grid.getHeight()); + + nav_grid::NavGridInfo bump_up_info = grid.getInfo(); + bump_up_info.origin_y = 2; + grid.updateInfo(bump_up_info); + checkUpdateGridValues(grid, 0, 7, 0, 3); + + nav_grid::NavGridInfo bump_left_info = grid.getInfo(); + bump_left_info.origin_x = -1; + grid.updateInfo(bump_left_info); + checkUpdateGridValues(grid, 4, grid.getWidth(), 0, 3); + + nav_grid::NavGridInfo bump_down_info = grid.getInfo(); + bump_down_info.origin_y = 0; + grid.updateInfo(bump_down_info); + checkUpdateGridValues(grid, 4, grid.getWidth(), 2, grid.getHeight()); + + + initializeTestGrid(grid); + nav_grid::NavGridInfo bump_far_right_info = grid.getInfo(); + bump_far_right_info.origin_x = 30; + grid.updateInfo(bump_far_right_info); + checkUpdateGridValues(grid, 0, 0, 0, 0); +} + +TEST(VectorNavGrid, combined_changes) +{ + // This is not a complete set of possible combined changes, just enough to satisfy my curiousity + nav_grid::VectorNavGrid grid; + initializeTestGrid(grid); + checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight()); + + nav_grid::NavGridInfo info1 = grid.getInfo(); + info1.width = 15; + info1.origin_x = -5.0; + grid.updateInfo(info1); + checkUpdateGridValues(grid, 5, grid.getWidth(), 0, grid.getHeight()); + + initializeTestGrid(grid); + nav_grid::NavGridInfo info2 = grid.getInfo(); + info2.width = 17; + info2.origin_x = -5.0; + grid.updateInfo(info2); + checkUpdateGridValues(grid, 5, grid.getWidth() - 2, 0, grid.getHeight()); + + initializeTestGrid(grid); + nav_grid::NavGridInfo info3 = grid.getInfo(); + info3.width = 2; + info3.origin_x = 2.0; + grid.updateInfo(info3); + checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight()); + + initializeTestGrid(grid); + nav_grid::NavGridInfo info4 = grid.getInfo(); + info4.width = 20; + info4.height = 20; + info4.origin_x = -2.0; + info4.origin_y = -5.0; + grid.updateInfo(info4); + checkUpdateGridValues(grid, 2, 12, 5, 10); +} + +TEST(Index, comparison_tests) +{ + unsigned int N = 5; + for (unsigned int x0 = 0; x0 < N; ++x0) + { + for (unsigned int y0 = 0; y0 < N; ++y0) + { + nav_grid::Index index0(x0, y0); + + for (unsigned int x1 = 0; x1 < N; ++x1) + { + for (unsigned int y1 = 0; y1 < N; ++y1) + { + nav_grid::Index index1(x1, y1); + // Check equality and the test for equality that sets use + // See https://stackoverflow.com/a/1114862 + if (x0 == x1 && y0 == y1) + { + EXPECT_EQ(index0, index1); + EXPECT_TRUE(!(index0 < index1) && !(index1 < index0)); + EXPECT_GE(index0, index1); + EXPECT_LE(index0, index1); + EXPECT_GE(index1, index0); + EXPECT_LE(index1, index0); + } + else + { + EXPECT_NE(index0, index1); + EXPECT_FALSE(!(index0 < index1) && !(index1 < index0)); + if (x0 < x1 || (x0 == x1 && y0 < y1)) + { + EXPECT_LT(index0, index1); + EXPECT_GT(index1, index0); + EXPECT_LE(index0, index1); + EXPECT_GE(index1, index0); + } + else + { + EXPECT_GT(index0, index1); + EXPECT_LT(index1, index0); + EXPECT_GE(index0, index1); + EXPECT_LE(index1, index0); + } + } + } + } + } + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}