commit 7f4eb6f1bfbbae2c8de61450173d17c8233aae5b Author: Hshine <1491134420@qq.com> Date: Thu Feb 22 20:23:25 2024 +0800 omni move gazebo simulate diff --git a/gazebo_model/construction_cone.zip b/gazebo_model/construction_cone.zip new file mode 100644 index 0000000..b3fe677 Binary files /dev/null and b/gazebo_model/construction_cone.zip differ diff --git a/gazebo_model/end_plane/materials/scripts/end_plane.material b/gazebo_model/end_plane/materials/scripts/end_plane.material new file mode 100644 index 0000000..97836ef --- /dev/null +++ b/gazebo_model/end_plane/materials/scripts/end_plane.material @@ -0,0 +1,17 @@ +material EndPlane/Image + { + technique + { + pass + { + ambient 1 1 1 1.000000 + diffuse 1 1 1 1.000000 + specular 0.03 0.03 0.03 1.000000 + + texture_unit + { + texture end.png + } + } + } + } diff --git a/gazebo_model/end_plane/materials/textures/background.png b/gazebo_model/end_plane/materials/textures/background.png new file mode 100644 index 0000000..a7550f4 Binary files /dev/null and b/gazebo_model/end_plane/materials/textures/background.png differ diff --git a/gazebo_model/end_plane/materials/textures/end.png b/gazebo_model/end_plane/materials/textures/end.png new file mode 100644 index 0000000..98568d2 Binary files /dev/null and b/gazebo_model/end_plane/materials/textures/end.png differ diff --git a/gazebo_model/end_plane/materials/textures/start.png b/gazebo_model/end_plane/materials/textures/start.png new file mode 100644 index 0000000..0ef18e0 Binary files /dev/null and b/gazebo_model/end_plane/materials/textures/start.png differ diff --git a/gazebo_model/end_plane/model.config b/gazebo_model/end_plane/model.config new file mode 100644 index 0000000..fb458b1 --- /dev/null +++ b/gazebo_model/end_plane/model.config @@ -0,0 +1,7 @@ + + + End Plane + 1.0 + model.sdf + My textured ground plane. + diff --git a/gazebo_model/end_plane/model.sdf b/gazebo_model/end_plane/model.sdf new file mode 100644 index 0000000..bd944e9 --- /dev/null +++ b/gazebo_model/end_plane/model.sdf @@ -0,0 +1,44 @@ + + + + true + + + + + + 0 0 1 + 0.6 0.6 0.0001 + + + + + + + 100 + 50 + + + + + + + false + + + 0 0 1 + 0.6 0.6 0.0001 + + + + + + + + + + diff --git a/gazebo_model/start_plane/materials/scripts/start_plane.material b/gazebo_model/start_plane/materials/scripts/start_plane.material new file mode 100644 index 0000000..10cca92 --- /dev/null +++ b/gazebo_model/start_plane/materials/scripts/start_plane.material @@ -0,0 +1,17 @@ +material StartPlane/Image + { + technique + { + pass + { + ambient 1 1 1 1.000000 + diffuse 1 1 1 1.000000 + specular 0.03 0.03 0.03 1.000000 + + texture_unit + { + texture start.png + } + } + } + } diff --git a/gazebo_model/start_plane/materials/textures/background.png b/gazebo_model/start_plane/materials/textures/background.png new file mode 100644 index 0000000..a7550f4 Binary files /dev/null and b/gazebo_model/start_plane/materials/textures/background.png differ diff --git a/gazebo_model/start_plane/materials/textures/end.png b/gazebo_model/start_plane/materials/textures/end.png new file mode 100644 index 0000000..98568d2 Binary files /dev/null and b/gazebo_model/start_plane/materials/textures/end.png differ diff --git a/gazebo_model/start_plane/materials/textures/start.png b/gazebo_model/start_plane/materials/textures/start.png new file mode 100644 index 0000000..0ef18e0 Binary files /dev/null and b/gazebo_model/start_plane/materials/textures/start.png differ diff --git a/gazebo_model/start_plane/model.config b/gazebo_model/start_plane/model.config new file mode 100644 index 0000000..af54692 --- /dev/null +++ b/gazebo_model/start_plane/model.config @@ -0,0 +1,7 @@ + + + Start Plane + 1.0 + model.sdf + My textured ground plane. + diff --git a/gazebo_model/start_plane/model.sdf b/gazebo_model/start_plane/model.sdf new file mode 100644 index 0000000..065e656 --- /dev/null +++ b/gazebo_model/start_plane/model.sdf @@ -0,0 +1,44 @@ + + + + true + + + + + + 0 0 1 + 0.5 1.8 0.0001 + + + + + + + 100 + 50 + + + + + + + false + + + 0 0 1 + 0.5 1.8 0.0001 + + + + + + + + + + diff --git a/omni_regulated_pure_pursuit_controller/CMakeLists.txt b/omni_regulated_pure_pursuit_controller/CMakeLists.txt new file mode 100644 index 0000000..9747073 --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.5) +project(omni_regulated_pure_pursuit_controller) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_util REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +nav2_package() +set(CMAKE_CXX_STANDARD 17) + +include_directories( + include +) + +set(dependencies + rclcpp + geometry_msgs + nav2_costmap_2d + pluginlib + nav_msgs + nav2_util + nav2_core + tf2 + tf2_geometry_msgs +) + +set(library_name omni_regulated_pure_pursuit_controller) + +add_library(${library_name} SHARED + src/regulated_pure_pursuit_controller.cpp) + +ament_target_dependencies(${library_name} + ${dependencies} +) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# # the following line skips the linter which checks for copyrights +# set(ament_cmake_copyright_FOUND TRUE) +# ament_lint_auto_find_test_dependencies() +# add_subdirectory(test) +# endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) + +pluginlib_export_plugin_description_file(nav2_core omni_regulated_pure_pursuit_controller.xml) + +ament_package() + diff --git a/omni_regulated_pure_pursuit_controller/README.md b/omni_regulated_pure_pursuit_controller/README.md new file mode 100644 index 0000000..ef2d348 --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/README.md @@ -0,0 +1,154 @@ +# Nav2 Omni Regulated Pure Pursuit Controller + +基于纯跟踪算法（Nav2 Regulated Pure Pursuit Controller）改写的适用于全向移动底盘的路经跟踪 + +以下为Nav2 Regulated Pure Pursuit Controller的readme文档 + +This is a controller (local trajectory planner) that implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. It also implements the basics behind the Adaptive Pure Pursuit algorithm to vary lookahead distances by current speed. It was developed by [Shrijit Singh](https://www.linkedin.com/in/shrijitsingh99/) and [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/) as part of the Nav2 working group. + +Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. + +This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). + +It builds on top of the ordinary pure pursuit algorithm in a number of ways. It also implements all the common variants of the pure pursuit algorithm such as adaptive pure pursuit. This controller is suitable for use on all types of robots, including differential, legged, and ackermann steering vehicles. It may also be used on omni-directional platforms, but won't be able to fully leverage the lateral movements of the base (you may consider DWB instead). + +This controller has been measured to run at well over 1 kHz on a modern intel processor. + +

+ +

+ +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-regulated-pp.html) for additional parameter descriptions. + +## Pure Pursuit Basics + +The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. + +In order to simply book-keeping, the global path is continuously pruned to the closest point to the robot (see the figure below) so that we only have to process useful path points. Then, the section of the path within the local costmap bounds is transformed to the robot frame and a lookahead point is determined using a predefined distance. + +Finally, the lookahead point will be given to the pure pursuit algorithm which finds the curvature of the path required to drive the robot to the lookahead point. This curvature is then applied to the velocity commands to allow the robot to drive. + +Note that a pure pursuit controller is that, it "purely" pursues the path without interest or concern about dynamic obstacles. Therefore, this controller should only be used when paired with a path planner that can generate a path the robot can follow. For a circular (or can be treated as circular) robot, this can really be any planner since you can leverage the particle / inflation relationship in planning. For a "large" robot for the environment or general non-circular robots, this must be something kinematically feasible, like the Smac Planner, such that the path is followable. + +![Lookahead algorithm](./doc/lookahead_algorithm.png) + +## Regulated Pure Pursuit Features + +We have created a new variation on the pure pursuit algorithm that we dubb the Regulated Pure Pursuit algorithm. We combine the features of the Adaptive Pure Pursuit algorithm with rules around linear velocity with a focus on consumer, industrial, and service robot's needs. We also implement several common-sense safety mechanisms like collision detection. + +The Regulated Pure Pursuit controller implements active collision detection. We use a parameter to set the maximum allowable time before a potential collision on the current velocity command. Using the current linear and angular velocity, we project forward in time that duration and check for collisions. Intuitively, you may think that collision checking between the robot and the lookahead point seems logical. However, if you're maneuvering in tight spaces, it makes alot of sense to only search forward a given amount of time to give the system a little leeway to get itself out. In confined spaces especially, we want to make sure that we're collision checking a reasonable amount of space for the current action being taken (e.g. if moving at 0.1 m/s, it makes no sense to look 10 meters ahead to the carrot, or 100 seconds into the future). This helps look further at higher speeds / angular rotations and closer with fine, slow motions in constrained environments so it doesn't over report collisions from valid motions near obstacles. If you set the maximum allowable to a large number, it will collision check all the way, but not exceeding, the lookahead point. We visualize the collision checking arc on the `lookahead_arc` topic. + +The regulated pure pursuit algorithm also makes use of the common variations on the pure pursuit algorithm. We implement the adaptive pure pursuit's main contribution of having velocity-scaled lookahead point distances. This helps make the controller more stable over a larger range of potential linear velocities. There are parameters for setting the lookahead gain (or lookahead time) and thresholded values for minimum and maximum. + +The final minor improvement we make is slowing on approach to the goal. Knowing that the optimal lookahead distance is `X`, we can take the difference in `X` and the actual distance of the lookahead point found to find the lookahead point error. During operations, the variation in this error should be exceptionally small and won't be triggered. However, at the end of the path, there are no more points at a lookahead distance away from the robot, so it uses the last point on the path. So as the robot approaches a target, its error will grow and the robot's velocity will be reduced proportional to this error until a minimum threshold. This is also tracked by the kinematic speed limits to ensure drivability. + +The major improvements that this work implements is the regulations on the linear velocity based on some cost functions. They were selected to remove long-standing bad behavior within the pure pursuit algorithm. Normal Pure Pursuit has an issue with overshoot and poor handling in particularly high curvature (or extremely rapidly changing curvature) environments. It is commonly known that this will cause the robot to overshoot from the path and potentially collide with the environment. These cost functions in the Regulated Pure Pursuit algorithm were also chosen based on common requirements and needs of mobile robots uses in service, commercial, and industrial use-cases; scaling by curvature creates intuitive behavior of slowing the robot when making sharp turns and slowing when its near a potential collision so that small variations don't clip obstacles. This is also really useful when working in partially observable environments (like turning in and out of aisles / hallways often) so that you slow before a sharp turn into an unknown dynamic environment to be more conservative in case something is in the way immediately requiring a stop. + +The cost functions penalize the robot's speed based on its proximity to obstacles and the curvature of the path. This is helpful to slow the robot when moving close to things in narrow spaces and scaling down the linear velocity by curvature helps to stabilize the controller over a larger range of lookahead point distances. This also has the added benefit of removing the sensitive tuning of the lookahead point / range, as the robot will track paths far better. Tuning is still required, but it is substantially easier to get reasonable behavior with minor adjustments. + +An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces. + +Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. + +Note: The maximum allowed time to collision is thresholded by the lookahead point, starting in Humble. This is such that collision checking isn't significantly overshooting the path, which can cause issues in constrained environments. For example, if there were a straight-line path going towards a wall that then turned left, if this parameter was set to high, then it would detect a collision past the point of actual robot intended motion. Thusly, if a robot is moving fast, selecting further out lookahead points is not only a matter of behavioral stability for Pure Pursuit, but also gives a robot further predictive collision detection capabilities. The max allowable time parameter is still in place for slow commands, as described in detail above. + +## Configuration + +| Parameter | Description | +|-----|----| +| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `lookahead_dist` | The lookahead distance to use to find the lookahead point | +| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | +| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | +| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | +| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | +| `transform_tolerance` | The TF transform tolerance | +| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | +| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | +| `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | +| `use_collision_detection` | Whether to enable collision detection. | +| `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions when `use_collision_detection` is `true`. It is limited to maximum distance of lookahead distance selected. | +| `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | +| `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | +| `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | +| `cost_scaling_gain` | A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within `cost_scaling_dist`. Lower value reduces speed more quickly. | +| `inflation_cost_scaling_factor` | The value of `cost_scaling_factor` set for the inflation layer in the local costmap. The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values | +| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii | +| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. | +| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | +| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | +| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | +| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | +| `use_interpolation` | Enables interpolation between poses on the path for lookahead point selection. Helps sparse paths to avoid inducing discontinuous commanded velocities. Set this to false for a potential performance boost, at the expense of smooth control. | + +Example fully-described XML with default parameter values: + +``` +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: "goal_checker" + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 1.0 + use_collision_detection: true + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + max_robot_pose_search_dist: 10.0 + use_interpolation: false + cost_scaling_dist: 0.3 + cost_scaling_gain: 1.0 + inflation_cost_scaling_factor: 3.0 +``` + +## Topics + +| Topic | Type | Description | +|-----|----|----| +| `lookahead_point` | `geometry_msgs/PointStamped` | The current lookahead point on the path | +| `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision_up_to_carrot`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | + +Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed guage. + +## Notes to users + +By default, the `use_cost_regulated_linear_velocity_scaling` is set to `false` because the generic sandbox environment we have setup is the TB3 world. This is a highly constrained environment so it overly triggers to slow the robot as everywhere has high costs. This is recommended to be set to `true` when not working in constantly high-cost spaces. + +To tune to get Adaptive Pure Pursuit behaviors, set all boolean parameters to false except `use_velocity_scaled_lookahead_dist` and make sure to tune `lookahead_time`, `min_lookahead_dist` and `max_lookahead_dist`. + +To tune to get Pure Pursuit behaviors, set all boolean parameters to false and make sure to tune `lookahead_dist`. + +Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way. + +The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it. diff --git a/omni_regulated_pure_pursuit_controller/doc/circle-segment-intersection.ipynb b/omni_regulated_pure_pursuit_controller/doc/circle-segment-intersection.ipynb new file mode 100644 index 0000000..5502387 --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/doc/circle-segment-intersection.ipynb @@ -0,0 +1,170 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "97dbdadd-7a94-4939-8ed5-c8551b662917", + "metadata": {}, + "source": [ + "# Circle Segment Intersection (for interpolation)\n", + "Here is an interactive plot that demonstrates the functionality of the formula to calculate the intersection of a line segment, and a circle centered at the origin." + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "d31dc723-a6dc-400d-8b31-fe84ea6d5e45", + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.jupyter.widget-view+json": { + "model_id": "cbfad4e8309a4ee2bef53994add83330", + "version_major": 2, + "version_minor": 0 + }, + "text/plain": [ + "VBox(children=(Label(value='A and B can be moved with the mouse. One must be inside the circle, and one must b…" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "from bqplot import *\n", + "import numpy as np\n", + "import ipywidgets as widgets\n", + "\n", + "\n", + "def circle_segment_intersection(p1, p2, r):\n", + " x1, y1 = p1\n", + " x2, y2 = p2\n", + " dx = x2 - x1\n", + " dy = y2 - y1\n", + " dr2 = dx ** 2 + dy ** 2\n", + " D = x1 * y2 - x2 * y1\n", + " d1 = x1 ** 2 + y1 ** 2\n", + " d2 = x2 ** 2 + y2 ** 2\n", + " dd = d2 - d1\n", + " sqrt_term = np.sqrt(r ** 2 * dr2 - D ** 2)\n", + " x = (D * dy + np.copysign(1.0, dd) * dx * sqrt_term) / dr2\n", + " y = (-D * dx + np.copysign(1.0, dd) * dy * sqrt_term) / dr2\n", + " return x, y\n", + "\n", + "\n", + "MAX = 5.0\n", + "x_sc = LinearScale(min=-MAX, max=MAX)\n", + "y_sc = LinearScale(min=-MAX, max=MAX)\n", + "\n", + "ax_x = Axis(label=\"x\", scale=x_sc, tick_format=\"0.0f\")\n", + "ax_y = Axis(label=\"y\", scale=y_sc, orientation=\"vertical\", tick_format=\"0.0f\")\n", + "\n", + "points = Scatter(\n", + " names=[\"A\", \"B\"], x=[0.0, 3.0], y=[2.0, 4.0], scales={\"x\": x_sc, \"y\": y_sc}, enable_move=True\n", + ")\n", + "\n", + "\n", + "def get_circle(r):\n", + " t = np.linspace(0, 2 * np.pi)\n", + " x = r * np.cos(t)\n", + " y = r * np.sin(t)\n", + " return x, y\n", + "\n", + "radius_slider = widgets.FloatSlider(min=0.0, max=MAX, value=3.0, description=\"Circle radius\")\n", + "circle_x, circle_y = get_circle(radius_slider.value)\n", + "\n", + "circle = Lines(x=circle_x, y=circle_y, scales={\"x\": x_sc, \"y\": y_sc}, colors=[\"green\"])\n", + "\n", + "x1, x2 = points.x\n", + "y1, y2 = points.y\n", + "xi, yi = circle_segment_intersection((x1, y1), (x2, y2), radius_slider.value)\n", + "\n", + "intersection = Scatter(\n", + " names=[\"C\"],\n", + " x=[xi],\n", + " y=[yi],\n", + " scales={\"x\": x_sc, \"y\": y_sc},\n", + " enable_move=False,\n", + " colors=[\"purple\"],\n", + ")\n", + "\n", + "fig = Figure(axes=[ax_x, ax_y], marks=[circle, points, intersection])\n", + "\n", + "fig.max_aspect_ratio = 1\n", + "fig.min_aspect_ratio = 1\n", + "\n", + "\n", + "def both_inside_or_both_outside_circle(points, r):\n", + " x1, x2 = points.x\n", + " y1, y2 = points.y\n", + " d1 = x1 ** 2 + y1 ** 2\n", + " d2 = x2 ** 2 + y2 ** 2\n", + " if d1 < r ** 2 and d2 < r ** 2:\n", + " return True\n", + " elif d1 > r ** 2 and d2 > r ** 2:\n", + " return True\n", + " else:\n", + " return False\n", + "\n", + "\n", + "def update_circle(message):\n", + " circle_x, circle_y = get_circle(radius_slider.value)\n", + " circle.x = circle_x\n", + " circle.y = circle_y\n", + " update_intersection(message)\n", + "\n", + "\n", + "def update_intersection(message):\n", + " x1, x2 = points.x\n", + " y1, y2 = points.y\n", + " r = radius_slider.value\n", + " if both_inside_or_both_outside_circle(points, r):\n", + " circle.colors = [\"red\"]\n", + " intersection.x = []\n", + " intersection.y = []\n", + " else:\n", + " circle.colors = [\"green\"]\n", + " xi, yi = circle_segment_intersection((x1, y1), (x2, y2), r)\n", + " intersection.x = [xi]\n", + " intersection.y = [yi]\n", + "\n", + "\n", + "points.observe(update_intersection, [\"x\", \"y\"])\n", + "\n", + "radius_slider.observe(update_circle, \"value\")\n", + "\n", + "widgets.VBox(\n", + " [\n", + " widgets.Label(\n", + " \"A and B can be moved with the mouse. One must be inside the circle, and one must be outside.\",\n", + " fixed=True,\n", + " ),\n", + " radius_slider,\n", + " fig,\n", + " ]\n", + ")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/omni_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png b/omni_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png new file mode 100644 index 0000000..8507ddd Binary files /dev/null and b/omni_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png differ diff --git a/omni_regulated_pure_pursuit_controller/include/omni_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/omni_regulated_pure_pursuit_controller/include/omni_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp new file mode 100644 index 0000000..5caf20c --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/include/omni_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -0,0 +1,326 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_core/controller.hpp" +#include "rclcpp/rclcpp.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace omni_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + * @brief Regulated pure pursuit controller plugin + */ +class RegulatedPurePursuitController : public nav2_core::Controller +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + RegulatedPurePursuitController() = default; + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + ~RegulatedPurePursuitController() override = default; + + /** + * @brief Configure controller state machine + * @param parent WeakPtr to node + * @param name Name of plugin + * @param tf TF buffer + * @param costmap_ros Costmap2DROS object of environment + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + /** + * @brief Cleanup controller state machine + */ + void cleanup() override; + + /** + * @brief Activate controller state machine + */ + void activate() override; + + /** + * @brief Deactivate controller state machine + */ + void deactivate() override; + + /** + * @brief Compute the best command given the current pose and velocity, with possible debug information + * + * Same as above computeVelocityCommands, but with debug results. + * If the results pointer is not null, additional information about the twists + * evaluated will be in results after the call. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands + * @return Best command + */ + geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * /*goal_checker*/) override; + + /** + * @brief nav2_core setPlan - Sets the global plan + * @param path The global plan + */ + void setPlan(const nav_msgs::msg::Path & path) override; + + /** + * @brief Limits the maximum linear speed of the robot. + * @param speed_limit expressed in absolute value (in m/s) + * or in percentage from maximum robot speed. + * @param percentage Setting speed limit in percentage if true + * or in absolute values in false case. + */ + void setSpeedLimit(const double & speed_limit, const bool & percentage) override; + +protected: + /** + * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint + * Points ineligible to be selected as a lookahead point if they are any of the following: + * - Outside the local_costmap (collision avoidance cannot be assured) + * @param pose pose to transform + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Transform a pose to another frame. + * @param frame Frame ID to transform to + * @param in_pose Pose input to transform + * @param out_pose transformed output + * @return bool if successful + */ + bool transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const; + + /** + * @brief Get lookahead distance + * @param cmd the current speed to use to compute lookahead point + * @return lookahead distance + */ + double getLookAheadDistance(const geometry_msgs::msg::Twist &); + + /** + * @brief Creates a PointStamped message for visualization + * @param carrot_pose Input carrot point as a PoseStamped + * @return CarrotMsg a carrot point marker, PointStamped + */ + std::unique_ptr createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @brief Whether robot should rotate to rough path heading + * @param carrot_pose current lookahead point + * @param angle_to_path Angle of robot output relatie to carrot marker + * @return Whether should rotate to path heading + */ + bool shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path); + + /** + * @brief Whether robot should rotate to final goal orientation + * @param carrot_pose current lookahead point + * @return Whether should rotate to goal heading + */ + bool shouldRotateToGoalHeading(const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @brief Create a smooth and kinematically smoothed rotation command + * @param linear_vel linear velocity + * @param angular_vel angular velocity + * @param angle_to_path Angle of robot output relatie to carrot marker + * @param curr_speed the current robot speed + */ + void rotateToHeading( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); + + /** + * @brief Whether collision is imminent + * @param robot_pose Pose of robot + * @param carrot_pose Pose of carrot + * @param linear_vel linear velocity to forward project + * @param angular_vel angular velocity to forward project + * @param carrot_dist Distance to the carrot for PP + * @return Whether collision is imminent + */ + bool isCollisionImminent( + const geometry_msgs::msg::PoseStamped &, + const double &, const double &, + const double &); + + /** + * @brief checks for collision at projected pose + * @param x Pose of pose x + * @param y Pose of pose y + * @param theta orientation of Yaw + * @return Whether in collision + */ + bool inCollision( + const double & x, + const double & y, + const double & theta); + /** + * @brief Cost at a point + * @param x Pose of pose x + * @param y Pose of pose y + * @return Cost of pose in costmap + */ + double costAtPose(const double & x, const double & y); + + double approachVelocityScalingFactor( + const nav_msgs::msg::Path & path + ) const; + + void applyApproachVelocityScaling( + const nav_msgs::msg::Path & path, + double & linear_vel + ) const; + + /** + * @brief apply regulation constraints to the system + * @param linear_vel robot command linear velocity input + * @param lookahead_dist optimal lookahead distance + * @param curvature curvature of path + * @param speed Speed of robot + * @param pose_cost cost at this pose + */ + void applyConstraints( + const double & curvature, const geometry_msgs::msg::Twist & speed, + const double & pose_cost, const nav_msgs::msg::Path & path, + double & linear_vel, double & sign); + + /** + * @brief Find the intersection a circle and a line segment. + * This assumes the circle is centered at the origin. + * If no intersection is found, a floating point error will occur. + * @param p1 first endpoint of line segment + * @param p2 second endpoint of line segment + * @param r radius of circle + * @return point of intersection + */ + static geometry_msgs::msg::Point circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r); + + /** + * @brief Get lookahead point + * @param lookahead_dist Optimal lookahead distance + * @param path Current global path + * @return Lookahead point + */ + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + + /** + * @brief checks for the cusp position + * @param pose Pose input to determine the cusp position + * @return robot distance from the cusp + */ + double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); + + /** + * Get the greatest extent of the costmap in meters from the center. + * @return max of distance from center in meters to edge of costmap + */ + double getCostmapMaxExtent() const; + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + std::shared_ptr tf_; + std::string plugin_name_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; + rclcpp::Clock::SharedPtr clock_; + + double desired_linear_vel_, base_desired_linear_vel_; + double lookahead_dist_; + double rotate_to_heading_angular_vel_; + double max_lookahead_dist_; + double min_lookahead_dist_; + double lookahead_time_; + bool use_velocity_scaled_lookahead_dist_; + tf2::Duration transform_tolerance_; + double min_approach_linear_velocity_; + double approach_velocity_scaling_dist_; + double control_duration_; + double max_allowed_time_to_collision_up_to_carrot_; + bool use_collision_detection_; + bool use_regulated_linear_velocity_scaling_; + bool use_cost_regulated_linear_velocity_scaling_; + double cost_scaling_dist_; + double cost_scaling_gain_; + double inflation_cost_scaling_factor_; + double regulated_linear_scaling_min_radius_; + double regulated_linear_scaling_min_speed_; + bool use_rotate_to_heading_; + double max_angular_accel_; + double rotate_to_heading_min_angle_; + double goal_dist_tol_; + bool allow_reversing_; + double max_robot_pose_search_dist_; + bool use_interpolation_; + + nav_msgs::msg::Path global_plan_; + std::shared_ptr> global_path_pub_; + std::shared_ptr> + carrot_pub_; + std::shared_ptr> carrot_arc_pub_; + std::unique_ptr> + collision_checker_; + + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/omni_regulated_pure_pursuit_controller/omni_regulated_pure_pursuit_controller.xml b/omni_regulated_pure_pursuit_controller/omni_regulated_pure_pursuit_controller.xml new file mode 100644 index 0000000..09f05d0 --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/omni_regulated_pure_pursuit_controller.xml @@ -0,0 +1,10 @@ + + + + + omni_regulated_pure_pursuit_controller + + + + + diff --git a/omni_regulated_pure_pursuit_controller/package.xml b/omni_regulated_pure_pursuit_controller/package.xml new file mode 100644 index 0000000..2012aed --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/package.xml @@ -0,0 +1,33 @@ + + + + omni_regulated_pure_pursuit_controller + 1.1.13 + omni_Regulated Pure Pursuit Controller + Steve Macenski + Shrijit Singh + Apache-2.0 + + ament_cmake + + nav2_common + nav2_core + nav2_util + nav2_costmap_2d + rclcpp + geometry_msgs + nav2_msgs + pluginlib + tf2 + tf2_geometry_msgs + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + + + + diff --git a/omni_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/omni_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp new file mode 100644 index 0000000..f72cf9d --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -0,0 +1,924 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "omni_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" +#include "nav2_core/exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" + +using std::hypot; +using std::min; +using std::max; +using std::abs; +using nav2_util::declare_parameter_if_not_declared; +using nav2_util::geometry_utils::euclidean_distance; +using namespace nav2_costmap_2d; // NOLINT +using rcl_interfaces::msg::ParameterType; + +namespace omni_regulated_pure_pursuit_controller +{ + +void RegulatedPurePursuitController::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) +{ + auto node = parent.lock(); + node_ = parent; + if (!node) { + throw nav2_core::PlannerException("Unable to lock node!"); + } + + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + tf_ = tf; + plugin_name_ = name; + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + double transform_tolerance = 0.1; + double control_frequency = 20.0; + goal_dist_tol_ = 0.25; // reasonable default before first update + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_collision_detection", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(getCostmapMaxExtent())); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_interpolation", + rclcpp::ParameterValue(true)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); + base_desired_linear_vel_ = desired_linear_vel_; + node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); + node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_angular_vel", + rotate_to_heading_angular_vel_); + node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); + node->get_parameter( + plugin_name_ + ".use_velocity_scaled_lookahead_dist", + use_velocity_scaled_lookahead_dist_); + node->get_parameter( + plugin_name_ + ".min_approach_linear_velocity", + min_approach_linear_velocity_); + node->get_parameter( + plugin_name_ + ".approach_velocity_scaling_dist", + approach_velocity_scaling_dist_); + if (approach_velocity_scaling_dist_ > costmap_->getSizeInMetersX() / 2.0) { + RCLCPP_WARN( + logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " + "leading to permanent slowdown"); + } + node->get_parameter( + plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + max_allowed_time_to_collision_up_to_carrot_); + node->get_parameter( + plugin_name_ + ".use_collision_detection", + use_collision_detection_); + node->get_parameter( + plugin_name_ + ".use_regulated_linear_velocity_scaling", + use_regulated_linear_velocity_scaling_); + node->get_parameter( + plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + use_cost_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_); + node->get_parameter(plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_); + node->get_parameter( + plugin_name_ + ".inflation_cost_scaling_factor", + inflation_cost_scaling_factor_); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_radius", + regulated_linear_scaling_min_radius_); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_speed", + regulated_linear_scaling_min_speed_); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); + node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); + node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_); + node->get_parameter("controller_frequency", control_frequency); + node->get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", + max_robot_pose_search_dist_); + node->get_parameter( + plugin_name_ + ".use_interpolation", + use_interpolation_); + + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + control_duration_ = 1.0 / control_frequency; + + if (inflation_cost_scaling_factor_ <= 0.0) { + RCLCPP_WARN( + logger_, "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; + } + + /** Possible to drive in reverse direction if and only if + "use_rotate_to_heading" parameter is set to false **/ + + if (use_rotate_to_heading_ && allow_reversing_) { + RCLCPP_WARN( + logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. By default setting use_rotate_to_heading true"); + allow_reversing_ = false; + } + + global_path_pub_ = node->create_publisher("received_global_plan", 1); + carrot_pub_ = node->create_publisher("lookahead_point", 1); + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); + + // initialize collision checker and set costmap + collision_checker_ = std::make_unique>(costmap_); + collision_checker_->setCostmap(costmap_); +} + +void RegulatedPurePursuitController::cleanup() +{ + RCLCPP_INFO( + logger_, + "Cleaning up controller: %s of type" + " regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_.reset(); + carrot_pub_.reset(); + carrot_arc_pub_.reset(); +} + +void RegulatedPurePursuitController::activate() +{ + RCLCPP_INFO( + logger_, + "Activating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_activate(); + carrot_pub_->on_activate(); + carrot_arc_pub_->on_activate(); + // Add callback for dynamic parameters + auto node = node_.lock(); + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &RegulatedPurePursuitController::dynamicParametersCallback, + this, std::placeholders::_1)); +} + +void RegulatedPurePursuitController::deactivate() +{ + RCLCPP_INFO( + logger_, + "Deactivating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_deactivate(); + carrot_pub_->on_deactivate(); + carrot_arc_pub_->on_deactivate(); + dyn_params_handler_.reset(); +} + +std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + auto carrot_msg = std::make_unique(); + carrot_msg->header = carrot_pose.header; + carrot_msg->point.x = carrot_pose.pose.position.x; + carrot_msg->point.y = carrot_pose.pose.position.y; + carrot_msg->point.z = 0.01; // publish right over map to stand out + return carrot_msg; +} + +double RegulatedPurePursuitController::getLookAheadDistance( + const geometry_msgs::msg::Twist & speed) +{ + // 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(speed.linear.x) * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } + + return lookahead_dist; +} + +geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & speed, + nav2_core::GoalChecker * goal_checker) +{ + std::lock_guard lock_reinit(mutex_); + + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + + // Update for the current goal checker's state + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist vel_tolerance; + if (!goal_checker->getTolerances(pose_tolerance, vel_tolerance)) { + RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!"); + } else { + goal_dist_tol_ = pose_tolerance.position.x; + } + + // Transform path to robot base frame + auto transformed_plan = transformGlobalPlan(pose); + + // Find look ahead distance and point on path and publish + double lookahead_dist = getLookAheadDistance(speed); + + // Check for reverse driving + if (allow_reversing_) { + // Cusp check + double dist_to_cusp = findVelocitySignChange(transformed_plan); + + // if the lookahead distance is further than the cusp, use the cusp distance instead + if (dist_to_cusp < lookahead_dist) { + lookahead_dist = dist_to_cusp; + } + } + + auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + carrot_pub_->publish(createCarrotMsg(carrot_pose)); + + double linear_vel;// angular_vel; + + // Find distance^2 to look ahead point (carrot) in robot base frame + //This is the chord length of the circle + const double carrot_dist2 = + (carrot_pose.pose.position.x * carrot_pose.pose.position.x) + + (carrot_pose.pose.position.y * carrot_pose.pose.position.y); + + //Find curvature of circle (k = 1 / R) + double curvature = 0.0; + if (carrot_dist2 > 0.001) { + curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; + } + + // // Setting the velocity direction + double sign = 1.0; + if (allow_reversing_) { + sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; + } + + linear_vel = desired_linear_vel_; + + applyConstraints( + curvature, speed, + costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, + linear_vel, sign); + + // Trans linear_vel to omni + const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); + + double sin_alpha = carrot_pose.pose.position.y / carrot_dist; + double cos_alpha = carrot_pose.pose.position.x / carrot_dist; + + double vx = linear_vel * cos_alpha; + double vy = linear_vel * sin_alpha; + + + // Make sure we're in compliance with basic constraints + // double angle_to_heading; + // if (shouldRotateToGoalHeading(carrot_pose)) { + // double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + // rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); + // } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { + // rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); + // } else { + // applyConstraints( + // curvature, speed, + // costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, + // linear_vel, sign); + + // // Apply curvature to angular velocity after constraining linear velocity + // angular_vel = linear_vel * curvature; + // } + + // Collision checking on this velocity heading + + // if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { + // throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!"); + // } + + // populate and return message + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header = pose.header; + cmd_vel.twist.linear.x = vx; + cmd_vel.twist.linear.y = vy; + cmd_vel.twist.angular.z = 0; + return cmd_vel; +} + +bool RegulatedPurePursuitController::shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) +{ + // Whether we should rotate robot to rough path heading + angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); + return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; +} + +bool RegulatedPurePursuitController::shouldRotateToGoalHeading( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + // Whether we should rotate robot to goal heading + double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); + return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_; +} + +void RegulatedPurePursuitController::rotateToHeading( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) +{ + // Rotate in place using max angular velocity / acceleration possible + linear_vel = 0.0; + const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; + angular_vel = sign * rotate_to_heading_angular_vel_; + + const double & dt = control_duration_; + const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; + const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; + angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); +} + +geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r) +{ + // Formula for intersection of a line with a circle centered at the origin, + // modified to always return the point that is on the segment between the two points. + // https://mathworld.wolfram.com/Circle-LineIntersection.html + // This works because the poses are transformed into the robot frame. + // This can be derived from solving the system of equations of a line and a circle + // which results in something that is just a reformulation of the quadratic formula. + // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at + // https://www.desmos.com/calculator/td5cwbuocd + double x1 = p1.x; + double x2 = p2.x; + double y1 = p1.y; + double y2 = p2.y; + + double dx = x2 - x1; + double dy = y2 - y1; + double dr2 = dx * dx + dy * dy; + double D = x1 * y2 - x2 * y1; + + // Augmentation to only return point within segment + double d1 = x1 * x1 + y1 * y1; + double d2 = x2 * x2 + y2 * y2; + double dd = d2 - d1; + + geometry_msgs::msg::Point p; + double sqrt_term = std::sqrt(r * r * dr2 - D * D); + p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2; + p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2; + return p; +} + +geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( + const double & lookahead_dist, + const nav_msgs::msg::Path & transformed_plan) +{ + // Find the first pose which is at a distance greater than the lookahead distance + auto goal_pose_it = std::find_if( + transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { + return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist; + }); + + // If the no pose is not far enough, take the last pose + if (goal_pose_it == transformed_plan.poses.end()) { + goal_pose_it = std::prev(transformed_plan.poses.end()); + } else if (use_interpolation_ && goal_pose_it != transformed_plan.poses.begin()) { + // Find the point on the line segment between the two poses + // that is exactly the lookahead distance away from the robot pose (the origin) + // This can be found with a closed form for the intersection of a segment and a circle + // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle, + // and goal_pose is guaranteed to be outside the circle. + auto prev_pose_it = std::prev(goal_pose_it); + auto point = circleSegmentIntersection( + prev_pose_it->pose.position, + goal_pose_it->pose.position, lookahead_dist); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = prev_pose_it->header.frame_id; + pose.header.stamp = goal_pose_it->header.stamp; + pose.pose.position = point; + return pose; + } + + return *goal_pose_it; +} + +bool RegulatedPurePursuitController::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const double & linear_vel, const double & angular_vel, + const double & carrot_dist) +{ + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // odom frame and the carrot_pose is in robot base frame. + + // check current point is OK + if (inCollision( + robot_pose.pose.position.x, robot_pose.pose.position.y, + tf2::getYaw(robot_pose.pose.orientation))) + { + return true; + } + + // visualization messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = robot_pose.header.stamp; + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = arc_pts_msg.header.frame_id; + pose_msg.header.stamp = arc_pts_msg.header.stamp; + + double projection_time = 0.0; + if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) { + // rotating to heading at goal or toward path + // Equation finds the angular distance required for the largest + // part of the robot radius to move to another costmap cell: + // theta_min = 2.0 * sin ((res/2) / r_max) + // via isosceles triangle r_max-r_max-resolution, + // dividing by angular_velocity gives us a timestep. + double max_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); + projection_time = + 2.0 * sin((costmap_->getResolution() / 2) / max_radius) / fabs(angular_vel); + } else { + // Normal path tracking + projection_time = costmap_->getResolution() / fabs(linear_vel); + } + + const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position; + geometry_msgs::msg::Pose2D curr_pose; + curr_pose.x = robot_pose.pose.position.x; + curr_pose.y = robot_pose.pose.position.y; + curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + + // only forward simulate within time requested + int i = 1; + while (i * projection_time < max_allowed_time_to_collision_up_to_carrot_) { + i++; + + // apply velocity at curr_pose over distance + curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += projection_time * angular_vel; + + // check if past carrot pose, where no longer a thoughtfully valid command + if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) { + break; + } + + // store it for visualization + pose_msg.pose.position.x = curr_pose.x; + pose_msg.pose.position.y = curr_pose.y; + pose_msg.pose.position.z = 0.01; + arc_pts_msg.poses.push_back(pose_msg); + + // check for collision at the projected pose + if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { + carrot_arc_pub_->publish(arc_pts_msg); + return true; + } + } + + carrot_arc_pub_->publish(arc_pts_msg); + + return false; +} + +bool RegulatedPurePursuitController::inCollision( + const double & x, + const double & y, + const double & theta) +{ + unsigned int mx, my; + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 30000, + "The dimensions of the costmap is too small to successfully check for " + "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " + "increase your costmap size."); + return false; + } + + double footprint_cost = collision_checker_->footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint()); + if (footprint_cost == static_cast(NO_INFORMATION) && + costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) + { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost >= static_cast(LETHAL_OBSTACLE); +} + +double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) +{ + unsigned int mx, my; + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_FATAL( + logger_, + "The dimensions of the costmap is too small to fully include your robot's footprint, " + "thusly the robot cannot proceed further"); + throw nav2_core::PlannerException( + "RegulatedPurePursuitController: Dimensions of the costmap are too small " + "to encapsulate the robot footprint at current speeds!"); + } + + unsigned char cost = costmap_->getCost(mx, my); + return static_cast(cost); +} + +double RegulatedPurePursuitController::approachVelocityScalingFactor( + const nav_msgs::msg::Path & transformed_path +) const +{ + // Waiting to apply the threshold based on integrated distance ensures we don't + // erroneously apply approach scaling on curvy paths that are contained in a large local costmap. + double remaining_distance = nav2_util::geometry_utils::calculate_path_length(transformed_path); + if (remaining_distance < approach_velocity_scaling_dist_) { + auto & last = transformed_path.poses.back(); + // Here we will use a regular euclidean distance from the robot frame (origin) + // to get smooth scaling, regardless of path density. + double distance_to_last_pose = std::hypot(last.pose.position.x, last.pose.position.y); + return distance_to_last_pose / approach_velocity_scaling_dist_; + } else { + return 1.0; + } +} + +void RegulatedPurePursuitController::applyApproachVelocityScaling( + const nav_msgs::msg::Path & path, + double & linear_vel +) const +{ + double approach_vel = linear_vel; + double velocity_scaling = approachVelocityScalingFactor(path); + 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 + linear_vel = std::min(linear_vel, approach_vel); +} + +void RegulatedPurePursuitController::applyConstraints( + const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, + const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + + // limit the linear velocity by curvature + const double radius = fabs(1.0 / curvature); + const double & min_rad = regulated_linear_scaling_min_radius_; + if (use_regulated_linear_velocity_scaling_ && radius < min_rad) { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + } + + // limit the linear velocity by proximity to obstacles + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(NO_INFORMATION) && + pose_cost != static_cast(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 / (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_; + } + } + + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + linear_vel = std::min(cost_vel, curvature_vel); + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + + applyApproachVelocityScaling(path, linear_vel); + + // Limit linear velocities to be valid + linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); + linear_vel = sign * linear_vel; +} + +void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; +} + +void RegulatedPurePursuitController::setSpeedLimit( + const double & speed_limit, + const bool & percentage) +{ + if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { + // Restore default value + desired_linear_vel_ = base_desired_linear_vel_; + } else { + if (percentage) { + // Speed limit is expressed in % from maximum speed of robot + desired_linear_vel_ = base_desired_linear_vel_ * speed_limit / 100.0; + } else { + // Speed limit is expressed in absolute value + desired_linear_vel_ = speed_limit; + } + } +} + +nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_.poses.empty()) { + throw nav2_core::PlannerException("Received plan with zero length"); + } + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + } + + // We'll discard points on the plan that are outside the local costmap + double max_costmap_extent = getCostmapMaxExtent(); + + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); + + // First find the closest pose on the path to the robot + // bounded by when the path turns around (if it does) so we don't get a pose from a later + // portion of the path + auto transformation_begin = + nav2_util::geometry_utils::min_by( + global_plan_.poses.begin(), closest_pose_upper_bound, + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // Find points up to max_transform_dist so we only transform them. + auto transformation_end = std::find_if( + transformation_begin, global_plan_.poses.end(), + [&](const auto & pose) { + return euclidean_distance(pose, robot_pose) > max_costmap_extent; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose); + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + global_path_pub_->publish(transformed_plan); + + if (transformed_plan.poses.empty()) { + throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +double RegulatedPurePursuitController::findVelocitySignChange( + const nav_msgs::msg::Path & transformed_plan) +{ + // Iterating through the transformed global path to determine the position of the cusp + for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = transformed_plan.poses[pose_id].pose.position.x - + transformed_plan.poses[pose_id - 1].pose.position.x; + double oa_y = transformed_plan.poses[pose_id].pose.position.y - + transformed_plan.poses[pose_id - 1].pose.position.y; + double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x - + transformed_plan.poses[pose_id].pose.position.x; + double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y - + transformed_plan.poses[pose_id].pose.position.y; + + /* Checking for the existance of cusp, in the path, using the dot product + and determine it's distance from the robot. If there is no cusp in the path, + then just determine the distance to the goal location. */ + if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) { + // returning the distance if there is a cusp + // The transformed path is in the robots frame, so robot is at the origin + return hypot( + transformed_plan.poses[pose_id].pose.position.x, + transformed_plan.poses[pose_id].pose.position.y); + } + } + + return std::numeric_limits::max(); +} + +bool RegulatedPurePursuitController::transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); + } + return false; +} + +double RegulatedPurePursuitController::getCostmapMaxExtent() const +{ + const double max_costmap_dim_meters = std::max( + costmap_->getSizeInMetersX(), costmap_->getSizeInMetersY()); + return max_costmap_dim_meters / 2.0; +} + + +rcl_interfaces::msg::SetParametersResult +RegulatedPurePursuitController::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(mutex_); + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".inflation_cost_scaling_factor") { + if (parameter.as_double() <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Ignoring parameter update."); + continue; + } + inflation_cost_scaling_factor_ = parameter.as_double(); + } else if (name == plugin_name_ + ".desired_linear_vel") { + desired_linear_vel_ = parameter.as_double(); + base_desired_linear_vel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_dist") { + lookahead_dist_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_lookahead_dist") { + max_lookahead_dist_ = parameter.as_double(); + } else if (name == plugin_name_ + ".min_lookahead_dist") { + min_lookahead_dist_ = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_time") { + lookahead_time_ = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + rotate_to_heading_angular_vel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".min_approach_linear_velocity") { + min_approach_linear_velocity_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { + max_allowed_time_to_collision_up_to_carrot_ = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_dist") { + cost_scaling_dist_ = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_gain") { + cost_scaling_gain_ = parameter.as_double(); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { + regulated_linear_scaling_min_radius_ = parameter.as_double(); + } else if (name == plugin_name_ + ".transform_tolerance") { + double transform_tolerance = parameter.as_double(); + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { + regulated_linear_scaling_min_speed_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_angular_accel") { + max_angular_accel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { + rotate_to_heading_min_angle_ = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { + use_velocity_scaled_lookahead_dist_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { + use_regulated_linear_velocity_scaling_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { + use_cost_regulated_linear_velocity_scaling_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_rotate_to_heading") { + if (parameter.as_bool() && allow_reversing_) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + use_rotate_to_heading_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".allow_reversing") { + if (use_rotate_to_heading_ && parameter.as_bool()) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + allow_reversing_ = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_regulated_pure_pursuit_controller + +// Register this controller as a nav2_core plugin +PLUGINLIB_EXPORT_CLASS( + omni_regulated_pure_pursuit_controller::RegulatedPurePursuitController, + nav2_core::Controller) diff --git a/omni_regulated_pure_pursuit_controller/test/CMakeLists.txt b/omni_regulated_pure_pursuit_controller/test/CMakeLists.txt new file mode 100644 index 0000000..3e8d4c5 --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -0,0 +1,15 @@ +# tests for regulated PP +ament_add_gtest(test_regulated_pp + test_regulated_pp.cpp + path_utils/path_utils.cpp +) +ament_target_dependencies(test_regulated_pp + ${dependencies} +) +target_link_libraries(test_regulated_pp + ${library_name} +) + +# Path utils test +ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp) +ament_target_dependencies(test_path_utils nav_msgs geometry_msgs tf2_geometry_msgs) diff --git a/omni_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp b/omni_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp new file mode 100644 index 0000000..4b516ec --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp @@ -0,0 +1,88 @@ +// Copyright (c) 2022 Adam Aposhian +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "path_utils.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace path_utils +{ + +void append_transform_to_path( + nav_msgs::msg::Path & path, + tf2::Transform & relative_transform) +{ + // Add a new empty pose + path.poses.emplace_back(); + // Get the previous, last pose (after the emplace_back so the reference isn't invalidated) + auto & previous_pose = *(path.poses.end() - 2); + auto & new_pose = path.poses.back(); + + // get map_transform of previous_pose + tf2::Transform map_transform; + tf2::fromMsg(previous_pose.pose, map_transform); + + tf2::Transform full_transform; + full_transform.mult(map_transform, relative_transform); + + tf2::toMsg(full_transform, new_pose.pose); + new_pose.header.frame_id = previous_pose.header.frame_id; +} + +void Straight::append(nav_msgs::msg::Path & path, double spacing) const +{ + auto num_points = std::floor(length_ / spacing); + path.poses.reserve(path.poses.size() + num_points); + tf2::Transform translation(tf2::Quaternion::getIdentity(), tf2::Vector3(spacing, 0.0, 0.0)); + for (size_t i = 1; i <= num_points; ++i) { + append_transform_to_path(path, translation); + } +} + +double chord_length(double radius, double radians) +{ + return 2 * radius * sin(radians / 2); +} + +void Arc::append(nav_msgs::msg::Path & path, double spacing) const +{ + double length = radius_ * std::abs(radians_); + size_t num_points = std::floor(length / spacing); + double radians_per_step = radians_ / num_points; + tf2::Transform transform( + tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), radians_per_step), + tf2::Vector3(chord_length(radius_, std::abs(radians_per_step)), 0.0, 0.0)); + path.poses.reserve(path.poses.size() + num_points); + for (size_t i = 0; i < num_points; ++i) { + append_transform_to_path(path, transform); + } +} + +nav_msgs::msg::Path generate_path( + geometry_msgs::msg::PoseStamped start, + double spacing, + std::initializer_list> segments) +{ + nav_msgs::msg::Path path; + path.header = start.header; + path.poses.push_back(start); + for (const auto & segment : segments) { + segment->append(path, spacing); + } + return path; +} + +} // namespace path_utils diff --git a/omni_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp b/omni_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp new file mode 100644 index 0000000..c079e21 --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2022 FireFly Automatix +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Adam Aposhian + +#ifndef PATH_UTILS__PATH_UTILS_HPP_ +#define PATH_UTILS__PATH_UTILS_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" + +namespace path_utils +{ + +/** + * Build human-readable test paths + */ +class PathSegment +{ +public: + virtual void append(nav_msgs::msg::Path & path, double spacing) const = 0; + virtual ~PathSegment() {} +}; + +class Arc : public PathSegment +{ +public: + explicit Arc(double radius, double radians) + : radius_(radius), radians_(radians) {} + void append(nav_msgs::msg::Path & path, double spacing) const override; + +private: + double radius_; + double radians_; +}; + +class Straight : public PathSegment +{ +public: + explicit Straight(double length) + : length_(length) {} + void append(nav_msgs::msg::Path & path, double spacing) const override; + +private: + double length_; +}; + +class LeftTurn : public Arc +{ +public: + explicit LeftTurn(double radius) + : Arc(radius, M_PI_2) {} +}; + +class RightTurn : public Arc +{ +public: + explicit RightTurn(double radius) + : Arc(radius, -M_PI_2) {} +}; + +class LeftTurnAround : public Arc +{ +public: + explicit LeftTurnAround(double radius) + : Arc(radius, M_PI) {} +}; + +class RightTurnAround : public Arc +{ +public: + explicit RightTurnAround(double radius) + : Arc(radius, -M_PI) {} +}; + +class LeftCircle : public Arc +{ +public: + explicit LeftCircle(double radius) + : Arc(radius, 2.0 * M_PI) {} +}; + +class RightCircle : public Arc +{ +public: + explicit RightCircle(double radius) + : Arc(radius, -2.0 * M_PI) {} +}; + +nav_msgs::msg::Path generate_path( + geometry_msgs::msg::PoseStamped start, + double spacing, + std::initializer_list> segments); + +} // namespace path_utils + +#endif // PATH_UTILS__PATH_UTILS_HPP_ diff --git a/omni_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp b/omni_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp new file mode 100644 index 0000000..7955555 --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp @@ -0,0 +1,128 @@ +// Copyright (c) 2022 Adam Aposhian +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "path_utils.hpp" +#include "gtest/gtest.h" + +using namespace path_utils; // NOLINT + +TEST(PathUtils, test_generate_straight) +{ + geometry_msgs::msg::PoseStamped start; + start.header.frame_id = "test_frame"; + + constexpr double path_length = 2.0; + constexpr double spacing = 1.0; + + auto path = generate_path( + start, spacing, { + std::make_unique(path_length) + }); + EXPECT_EQ(path.poses.size(), 3u); + for (const auto & pose : path.poses) { + EXPECT_EQ(pose.header.frame_id, start.header.frame_id); + } + EXPECT_DOUBLE_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(path.poses[0].pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(path.poses[0].pose.position.z, 0.0); + + EXPECT_NEAR(path.poses[1].pose.position.x, 1.0, 0.1); + EXPECT_NEAR(path.poses[1].pose.position.y, 0.0, 0.1); + EXPECT_NEAR(path.poses[1].pose.position.z, 0.0, 0.1); + + EXPECT_NEAR(path.poses[2].pose.position.x, 2.0, 0.1); + EXPECT_NEAR(path.poses[2].pose.position.y, 0.0, 0.1); + EXPECT_NEAR(path.poses[2].pose.position.z, 0.0, 0.1); +} + +TEST(PathUtils, test_half_turn) +{ + // Start at a more interesting place, turned the other way + geometry_msgs::msg::PoseStamped start; + start.header.frame_id = "map"; + start.pose.position.x = 1.0; + start.pose.position.y = -1.0; + start.pose.orientation.x = 0.0; + start.pose.orientation.y = 0.0; + start.pose.orientation.z = 1.0; + start.pose.orientation.w = 0.0; + + constexpr double spacing = 0.1; + constexpr double radius = 2.0; + + auto path = generate_path( + start, spacing, { + std::make_unique(radius), + }); + constexpr double expected_path_length = M_PI * radius; + EXPECT_NEAR(path.poses.size(), 1 + static_cast(expected_path_length / spacing), 10); + for (const auto & pose : path.poses) { + EXPECT_EQ(pose.header.frame_id, start.header.frame_id); + } + + // Check the last pose + auto & last_pose = path.poses.back(); + auto & last_position = last_pose.pose.position; + EXPECT_NEAR(last_position.x, 1.0, 0.2); + EXPECT_NEAR(last_position.y, 3.0, 0.2); + EXPECT_DOUBLE_EQ(last_position.z, 0.0); + + // Should be facing forward now + auto & last_orientation = last_pose.pose.orientation; + EXPECT_NEAR(last_orientation.x, 0.0, 0.1); + EXPECT_NEAR(last_orientation.y, 0.0, 0.1); + EXPECT_NEAR(last_orientation.z, 0.0, 0.1); + EXPECT_NEAR(last_orientation.w, 1.0, 0.1); +} + +TEST(PathUtils, test_generate_all) +{ + geometry_msgs::msg::PoseStamped start; + start.header.frame_id = "map"; + + constexpr double spacing = 0.1; + + auto path = generate_path( + start, spacing, { + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0, 2 * M_PI), // another circle + }); + constexpr double expected_path_length = 1.0 + 2.0 * (M_PI_2 + M_PI_2) + 2.0 * (M_PI) +3.0 * + (2.0 * M_PI); + EXPECT_NEAR(path.poses.size(), 1 + static_cast(expected_path_length / spacing), 50); + for (const auto & pose : path.poses) { + EXPECT_EQ(pose.header.frame_id, start.header.frame_id); + } + + // Check the last pose + auto & last_pose = path.poses.back(); + auto & last_position = last_pose.pose.position; + EXPECT_NEAR(last_position.x, 3.0, 0.5); + EXPECT_NEAR(last_position.y, 6.0, 0.5); + EXPECT_DOUBLE_EQ(last_position.z, 0.0); + + auto & last_orientation = last_pose.pose.orientation; + EXPECT_NEAR(last_orientation.x, 0.0, 0.1); + EXPECT_NEAR(last_orientation.y, 0.0, 0.1); + EXPECT_NEAR(last_orientation.z, 0.0, 0.1); + EXPECT_NEAR(last_orientation.w, 1.0, 0.1); +} diff --git a/omni_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/omni_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp new file mode 100644 index 0000000..83d814d --- /dev/null +++ b/omni_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -0,0 +1,991 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "path_utils/path_utils.hpp" +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_core/exceptions.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController +{ +public: + BasicAPIRPP() + : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {} + + nav_msgs::msg::Path getPlan() {return global_plan_;} + + double getSpeed() {return desired_linear_vel_;} + + std::unique_ptr createCarrotMsgWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return createCarrotMsg(carrot_pose); + } + + void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;} + void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;} + void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;} + + double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist) + { + return getLookAheadDistance(twist); + } + + static geometry_msgs::msg::Point circleSegmentIntersectionWrapper( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r) + { + return circleSegmentIntersection(p1, p2, r); + } + + geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( + const double & dist, const nav_msgs::msg::Path & path) + { + return getLookAheadPoint(dist, path); + } + + bool shouldRotateToPathWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) + { + return shouldRotateToPath(carrot_pose, angle_to_path); + } + + bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return shouldRotateToGoalHeading(carrot_pose); + } + + void rotateToHeadingWrapper( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) + { + return rotateToHeading(linear_vel, angular_vel, angle_to_path, curr_speed); + } + + void applyConstraintsWrapper( + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) + { + return applyConstraints( + curvature, curr_speed, pose_cost, path, + linear_vel, sign); + } + + double findVelocitySignChangeWrapper( + const nav_msgs::msg::Path & transformed_plan) + { + return findVelocitySignChange(transformed_plan); + } + + nav_msgs::msg::Path transformGlobalPlanWrapper( + const geometry_msgs::msg::PoseStamped & pose) + { + return transformGlobalPlan(pose); + } +}; + +TEST(RegulatedPurePursuitTest, basicAPI) +{ + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + + // instantiate + auto ctrl = std::make_shared(); + costmap->on_configure(rclcpp_lifecycle::State()); + ctrl->configure(node, name, tf, costmap); + ctrl->activate(); + ctrl->deactivate(); + ctrl->cleanup(); + + // setPlan and get plan + nav_msgs::msg::Path path; + path.poses.resize(2); + path.poses[0].header.frame_id = "fake_frame"; + ctrl->setPlan(path); + EXPECT_EQ(ctrl->getPlan().poses.size(), 2ul); + EXPECT_EQ(ctrl->getPlan().poses[0].header.frame_id, std::string("fake_frame")); + + // set speed limit + const double base_speed = ctrl->getSpeed(); + EXPECT_EQ(ctrl->getSpeed(), base_speed); + ctrl->setSpeedLimit(0.51, false); + EXPECT_EQ(ctrl->getSpeed(), 0.51); + ctrl->setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, false); + EXPECT_EQ(ctrl->getSpeed(), base_speed); + ctrl->setSpeedLimit(30, true); + EXPECT_EQ(ctrl->getSpeed(), base_speed * 0.3); + ctrl->setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, true); + EXPECT_EQ(ctrl->getSpeed(), base_speed); +} + +TEST(RegulatedPurePursuitTest, createCarrotMsg) +{ + auto ctrl = std::make_shared(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "Hi!"; + pose.pose.position.x = 1.0; + pose.pose.position.y = 12.0; + pose.pose.orientation.w = 0.5; + + auto rtn = ctrl->createCarrotMsgWrapper(pose); + EXPECT_EQ(rtn->header.frame_id, std::string("Hi!")); + EXPECT_EQ(rtn->point.x, 1.0); + EXPECT_EQ(rtn->point.y, 12.0); + EXPECT_EQ(rtn->point.z, 0.01); +} + +TEST(RegulatedPurePursuitTest, findVelocitySignChange) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPPfindVelocitySignChange"); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "smb"; + auto time = node->get_clock()->now(); + pose.header.stamp = time; + pose.pose.position.x = 1.0; + pose.pose.position.y = 0.0; + + nav_msgs::msg::Path path; + path.poses.resize(3); + path.header.frame_id = "smb"; + path.header.stamp = pose.header.stamp; + path.poses[0].pose.position.x = 1.0; + path.poses[0].pose.position.y = 1.0; + path.poses[1].pose.position.x = 2.0; + path.poses[1].pose.position.y = 2.0; + path.poses[2].pose.position.x = -1.0; + path.poses[2].pose.position.y = -1.0; + ctrl->setPlan(path); + auto rtn = ctrl->findVelocitySignChangeWrapper(path); + EXPECT_EQ(rtn, sqrt(8.0)); + + path.poses[2].pose.position.x = 3.0; + path.poses[2].pose.position.y = 3.0; + ctrl->setPlan(path); + rtn = ctrl->findVelocitySignChangeWrapper(path); + EXPECT_EQ(rtn, std::numeric_limits::max()); +} + +using CircleSegmentIntersectionParam = std::tuple< + std::pair, + std::pair, + double, + std::pair +>; + +class CircleSegmentIntersectionTest + : public ::testing::TestWithParam +{}; + +TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection) +{ + auto pair1 = std::get<0>(GetParam()); + auto pair2 = std::get<1>(GetParam()); + auto r = std::get<2>(GetParam()); + auto expected_pair = std::get<3>(GetParam()); + auto pair_to_point = [](std::pair p) -> geometry_msgs::msg::Point { + geometry_msgs::msg::Point point; + point.x = p.first; + point.y = p.second; + point.z = 0.0; + return point; + }; + auto p1 = pair_to_point(pair1); + auto p2 = pair_to_point(pair2); + auto actual = BasicAPIRPP::circleSegmentIntersectionWrapper(p1, p2, r); + auto expected_point = pair_to_point(expected_pair); + EXPECT_DOUBLE_EQ(actual.x, expected_point.x); + EXPECT_DOUBLE_EQ(actual.y, expected_point.y); + // Expect that the intersection point is actually r away from the origin + EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y)); +} + +INSTANTIATE_TEST_SUITE_P( + InterpolationTest, + CircleSegmentIntersectionTest, + testing::Values( + // Origin to the positive X axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {2.0, 0.0}, + 1.0, + {1.0, 0.0} +}, + // Origin to hte negative X axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-2.0, 0.0}, + 1.0, + {-1.0, 0.0} +}, + // Origin to the positive Y axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, 2.0}, + 1.0, + {0.0, 1.0} +}, + // Origin to the negative Y axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, -2.0}, + 1.0, + {0.0, -1.0} +}, + // non-origin to the X axis with non-unit circle, with the second point inside + CircleSegmentIntersectionParam{ + {4.0, 0.0}, + {-1.0, 0.0}, + 2.0, + {2.0, 0.0} +}, + // non-origin to the Y axis with non-unit circle, with the second point inside + CircleSegmentIntersectionParam{ + {0.0, 4.0}, + {0.0, -0.5}, + 2.0, + {0.0, 2.0} +}, + // origin to the positive X axis, on the circle + CircleSegmentIntersectionParam{ + {2.0, 0.0}, + {0.0, 0.0}, + 2.0, + {2.0, 0.0} +}, + // origin to the positive Y axis, on the circle + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, 2.0}, + 2.0, + {0.0, 2.0} +}, + // origin to the upper-right quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {6.0, 8.0}, + 5.0, + {3.0, 4.0} +}, + // origin to the lower-left quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-6.0, -8.0}, + 5.0, + {-3.0, -4.0} +}, + // origin to the upper-left quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-6.0, 8.0}, + 5.0, + {-3.0, 4.0} +}, + // origin to the lower-right quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {6.0, -8.0}, + 5.0, + {3.0, -4.0} +} +)); + +TEST(RegulatedPurePursuitTest, lookaheadAPI) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + ctrl->configure(node, name, tf, costmap); + + geometry_msgs::msg::Twist twist; + + // test getLookAheadDistance + double rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); // default lookahead_dist + + // shouldn't be a function of speed + twist.linear.x = 10.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); + + // now it should be a function of velocity, max out + ctrl->setVelocityScaledLookAhead(); + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.9); // 10 speed maxes out at max_lookahead_dist + + // check normal range + twist.linear.x = 0.35; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_NEAR(rtn, 0.525, 0.0001); // 1.5 * 0.35 + + // check minimum range + twist.linear.x = 0.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.3); + + // test getLookAheadPoint + double dist = 1.0; + nav_msgs::msg::Path path; + path.poses.resize(10); + for (uint i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = static_cast(i); + } + + // test exact hits + auto pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 1.0); + + // test getting next closest point without interpolation + node->set_parameter( + rclcpp::Parameter( + name + ".use_interpolation", + rclcpp::ParameterValue(false))); + ctrl->configure(node, name, tf, costmap); + dist = 3.8; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 4.0); + + // test end of path + dist = 100.0; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 9.0); + + // test interpolation + node->set_parameter( + rclcpp::Parameter( + name + ".use_interpolation", + rclcpp::ParameterValue(true))); + ctrl->configure(node, name, tf, costmap); + dist = 3.8; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 3.8); +} + +TEST(RegulatedPurePursuitTest, rotateTests) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + ctrl->configure(node, name, tf, costmap); + + // shouldRotateToPath + geometry_msgs::msg::PoseStamped carrot; + double angle_to_path_rtn; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 0.25; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 1.0; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), true); + + // shouldRotateToGoalHeading + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.0; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.24; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.26; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); + + // rotateToHeading + double lin_v = 10.0; + double ang_v = 0.5; + double angle_to_path = 0.4; + geometry_msgs::msg::Twist curr_speed; + curr_speed.angular.z = 1.75; + + // basic full speed at a speed + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(lin_v, 0.0); + EXPECT_EQ(ang_v, 1.8); + + // negative direction + angle_to_path = -0.4; + curr_speed.angular.z = -1.75; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(ang_v, -1.8); + + // kinematic clamping, no speed, some speed accelerating, some speed decelerating + angle_to_path = 0.4; + curr_speed.angular.z = 0.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 0.16, 0.01); + + curr_speed.angular.z = 1.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 1.16, 0.01); + + angle_to_path = -0.4; + curr_speed.angular.z = 1.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 0.84, 0.01); +} + +TEST(RegulatedPurePursuitTest, applyConstraints) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + + constexpr double approach_velocity_scaling_dist = 0.6; + nav2_util::declare_parameter_if_not_declared( + node, + name + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(approach_velocity_scaling_dist)); + + ctrl->configure(node, name, tf, costmap); + + auto no_approach_path = path_utils::generate_path( + geometry_msgs::msg::PoseStamped(), 0.1, { + std::make_unique(approach_velocity_scaling_dist + 1.0) + }); + + double curvature = 0.5; + geometry_msgs::msg::Twist curr_speed; + double pose_cost = 0.0; + double linear_vel = 0.0; + double sign = 1.0; + + // test curvature regulation (default) + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper( + curvature, curr_speed, pose_cost, no_approach_path, + linear_vel, sign); + EXPECT_EQ(linear_vel, 0.25); // min set speed + + linear_vel = 1.0; + curvature = 0.7407; + curr_speed.linear.x = 0.5; + ctrl->applyConstraintsWrapper( + curvature, curr_speed, pose_cost, no_approach_path, + linear_vel, sign); + EXPECT_NEAR(linear_vel, 0.5, 0.01); // lower by curvature + + linear_vel = 1.0; + curvature = 1000.0; + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper( + curvature, curr_speed, pose_cost, no_approach_path, + linear_vel, sign); + EXPECT_NEAR(linear_vel, 0.25, 0.01); // min out by curvature + + // Approach velocity scaling on a path with no distance left + auto approach_path = path_utils::generate_path( + geometry_msgs::msg::PoseStamped(), 0.1, { + std::make_unique(0.0) + }); + + linear_vel = 1.0; + curvature = 0.0; + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper( + curvature, curr_speed, pose_cost, approach_path, + linear_vel, sign); + EXPECT_NEAR(linear_vel, 0.05, 0.01); // min out on min approach velocity + + // now try with cost regulation (turn off velocity and only cost) + // ctrl->setCostRegulationScaling(); + // ctrl->resetVelocityRegulationScaling(); + // curvature = 0.0; + + // min changable cost + // pose_cost = 1; + // linear_vel = 0.5; + // curr_speed.linear.x = 0.5; + // ctrl->applyConstraintsWrapper( + // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.498, 0.01); + + // max changing cost + // pose_cost = 127; + // curr_speed.linear.x = 0.255; + // ctrl->applyConstraintsWrapper( + // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.255, 0.01); + + // over max cost thresh + // pose_cost = 200; + // curr_speed.linear.x = 0.25; + // ctrl->applyConstraintsWrapper( + // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.25, 0.01); + + // test kinematic clamping + // pose_cost = 200; + // curr_speed.linear.x = 1.0; + // ctrl->applyConstraintsWrapper( + // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.5, 0.01); +} + +TEST(RegulatedPurePursuitTest, testDynamicParameter) +{ + auto node = std::make_shared("Smactest"); + auto costmap = std::make_shared("global_costmap"); + costmap->on_configure(rclcpp_lifecycle::State()); + auto ctrl = + std::make_unique(); + auto tf = std::make_shared(node->get_clock()); + ctrl->configure(node, "test", tf, costmap); + ctrl->activate(); + + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.desired_linear_vel", 1.0), + rclcpp::Parameter("test.lookahead_dist", 7.0), + rclcpp::Parameter("test.max_lookahead_dist", 7.0), + rclcpp::Parameter("test.min_lookahead_dist", 6.0), + rclcpp::Parameter("test.lookahead_time", 1.8), + rclcpp::Parameter("test.rotate_to_heading_angular_vel", 18.0), + rclcpp::Parameter("test.min_approach_linear_velocity", 1.0), + rclcpp::Parameter("test.max_allowed_time_to_collision_up_to_carrot", 2.0), + rclcpp::Parameter("test.cost_scaling_dist", 2.0), + rclcpp::Parameter("test.cost_scaling_gain", 4.0), + rclcpp::Parameter("test.regulated_linear_scaling_min_radius", 10.0), + rclcpp::Parameter("test.transform_tolerance", 30.0), + rclcpp::Parameter("test.max_angular_accel", 3.0), + rclcpp::Parameter("test.rotate_to_heading_min_angle", 0.7), + rclcpp::Parameter("test.regulated_linear_scaling_min_speed", 4.0), + rclcpp::Parameter("test.use_velocity_scaled_lookahead_dist", false), + rclcpp::Parameter("test.use_regulated_linear_velocity_scaling", false), + rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false), + rclcpp::Parameter("test.allow_reversing", false), + rclcpp::Parameter("test.use_rotate_to_heading", false)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(node->get_parameter("test.desired_linear_vel").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("test.lookahead_dist").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.max_lookahead_dist").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.min_lookahead_dist").as_double(), 6.0); + EXPECT_EQ(node->get_parameter("test.lookahead_time").as_double(), 1.8); + EXPECT_EQ(node->get_parameter("test.rotate_to_heading_angular_vel").as_double(), 18.0); + EXPECT_EQ(node->get_parameter("test.min_approach_linear_velocity").as_double(), 1.0); + EXPECT_EQ( + node->get_parameter( + "test.max_allowed_time_to_collision_up_to_carrot").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.cost_scaling_dist").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.cost_scaling_gain").as_double(), 4.0); + EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_radius").as_double(), 10.0); + EXPECT_EQ(node->get_parameter("test.transform_tolerance").as_double(), 30.0); + EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 3.0); + EXPECT_EQ(node->get_parameter("test.rotate_to_heading_min_angle").as_double(), 0.7); + EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_speed").as_double(), 4.0); + EXPECT_EQ(node->get_parameter("test.use_velocity_scaled_lookahead_dist").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.use_regulated_linear_velocity_scaling").as_bool(), false); + EXPECT_EQ( + node->get_parameter( + "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false); +} + +class TransformGlobalPlanTest : public ::testing::Test +{ +protected: + void SetUp() override + { + ctrl_ = std::make_shared(); + node_ = std::make_shared("testRPP"); + tf_buffer_ = std::make_shared(node_->get_clock()); + costmap_ = std::make_shared("fake_costmap"); + } + + void configure_costmap(uint16_t width, double resolution) + { + constexpr char costmap_frame[] = "test_costmap_frame"; + constexpr char robot_frame[] = "test_robot_frame"; + + auto results = costmap_->set_parameters( + { + rclcpp::Parameter("global_frame", costmap_frame), + rclcpp::Parameter("robot_base_frame", robot_frame), + rclcpp::Parameter("width", width), + rclcpp::Parameter("height", width), + rclcpp::Parameter("resolution", resolution) + }); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + + rclcpp_lifecycle::State state; + costmap_->on_configure(state); + } + + void configure_controller(double max_robot_pose_search_dist) + { + std::string plugin_name = "test_rpp"; + nav2_util::declare_parameter_if_not_declared( + node_, plugin_name + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(max_robot_pose_search_dist)); + ctrl_->configure(node_, plugin_name, tf_buffer_, costmap_); + } + + void setup_transforms(geometry_msgs::msg::Point & robot_position) + { + transform_time_ = node_->get_clock()->now(); + // Note: transforms go parent to child + + // We will have a separate path and costmap frame for completeness, + // but we will leave them cooincident for convenience. + geometry_msgs::msg::TransformStamped path_to_costmap; + path_to_costmap.header.frame_id = PATH_FRAME; + path_to_costmap.header.stamp = transform_time_; + path_to_costmap.child_frame_id = COSTMAP_FRAME; + path_to_costmap.transform.translation.x = 0.0; + path_to_costmap.transform.translation.y = 0.0; + path_to_costmap.transform.translation.z = 0.0; + + geometry_msgs::msg::TransformStamped costmap_to_robot; + costmap_to_robot.header.frame_id = COSTMAP_FRAME; + costmap_to_robot.header.stamp = transform_time_; + costmap_to_robot.child_frame_id = ROBOT_FRAME; + costmap_to_robot.transform.translation.x = robot_position.x; + costmap_to_robot.transform.translation.y = robot_position.y; + costmap_to_robot.transform.translation.z = robot_position.z; + + tf2_msgs::msg::TFMessage tf_message; + tf_message.transforms = { + path_to_costmap, + costmap_to_robot + }; + for (const auto & transform : tf_message.transforms) { + tf_buffer_->setTransform(transform, "test", false); + } + tf_buffer_->setUsingDedicatedThread(true); // lying to let it do transforms + } + + static constexpr char PATH_FRAME[] = "test_path_frame"; + static constexpr char COSTMAP_FRAME[] = "test_costmap_frame"; + static constexpr char ROBOT_FRAME[] = "test_robot_frame"; + + std::shared_ptr ctrl_; + std::shared_ptr node_; + std::shared_ptr costmap_; + std::shared_ptr tf_buffer_; + rclcpp::Time transform_time_; +}; + +// This tests that not only should nothing get pruned on a costmap +// that contains the entire global_plan, and also that it doesn't skip to the end of the path +// which is closer to the robot pose than the start. +TEST_F(TransformGlobalPlanTest, no_pruning_on_large_costmap) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // A really big costmap + // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); + configure_controller(5.0); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + constexpr double spacing = 0.1; + constexpr double circle_radius = 1.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_EQ(transformed_plan.poses.size(), global_plan.poses.size()); +} + +// This plan shouldn't get pruned because of the costmap, +// but should be half pruned because it is halfway around the circle +TEST_F(TransformGlobalPlanTest, transform_start_selection) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 4.0; // on the other side of the circle + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 0.1; + constexpr double circle_radius = 2.0; // diameter 4 + + // A really big costmap + // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); + // This should just be at least half the circumference: pi*r ~= 6 + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_NEAR(transformed_plan.poses.size(), global_plan.poses.size() / 2, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} + +// This should throw an exception when all poses are outside of the costmap +TEST_F(TransformGlobalPlanTest, all_poses_outside_of_costmap) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = 1000.0; + robot_pose.pose.position.y = 1000.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 0.1; + constexpr double circle_radius = 2.0; // diameter 4 + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(10u, 0.1); + // This should just be at least half the circumference: pi*r ~= 6 + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + EXPECT_THROW(ctrl_->transformGlobalPlanWrapper(robot_pose), nav2_core::PlannerException); +} + +// Should shortcut the circle if the circle is shorter than max_robot_pose_search_dist +TEST_F(TransformGlobalPlanTest, good_circle_shortcut) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 0.1; + constexpr double circle_radius = 2.0; // diameter 4 + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); + // This should just be at least the circumference: 2*pi*r ~= 12 + constexpr double max_robot_pose_search_dist = 15.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_NEAR(transformed_plan.poses.size(), 1, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} + +// Simple costmap pruning on a straight line +TEST_F(TransformGlobalPlanTest, costmap_pruning) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 1.0; + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(20u, 0.5); + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + constexpr double path_length = 100.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(path_length) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_NEAR(transformed_plan.poses.size(), 10u, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} + +// Should prune out later portions of the path that come back into the costmap +TEST_F(TransformGlobalPlanTest, prune_after_leaving_costmap) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 1.0; + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(20u, 0.5); + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + constexpr double path_length = 100.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(path_length), + std::make_unique(1.0), + std::make_unique(path_length) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + // This should be essentially the same as the regular straight path + EXPECT_NEAR(transformed_plan.poses.size(), 10u, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} diff --git a/plc_bringup/CHANGELOG.rst b/plc_bringup/CHANGELOG.rst new file mode 100644 index 0000000..e69de29 diff --git a/plc_bringup/CMakeLists.txt b/plc_bringup/CMakeLists.txt new file mode 100644 index 0000000..ad76152 --- /dev/null +++ b/plc_bringup/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.5) +project(plc_bringup) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(navigation2 REQUIRED) + +nav2_package() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/plc_bringup/README.md b/plc_bringup/README.md new file mode 100644 index 0000000..cd6abb0 --- /dev/null +++ b/plc_bringup/README.md @@ -0,0 +1,60 @@ +# plc_bringup + +基于nav2_bringup改写的全向移动底盘gazebo仿真及导航（麦克纳姆轮） + +启动： +``` +source /opt/ros/humble/setup.bash +source ./install/setup.bash + +ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False +``` +> +* 运行roslaunch前，请将gazebo_model下start_plane、end_plane文件夹复制到.gazebo/models目录下 +* 若打开的小车赛道中没有出现锥筒请按如下操作： +将construction_cone.zip压缩包解压，然后将解压后的construction_cone文件复制到.gazebo/models目录下，construction_cone文件为锥筒的模型包，复制后运行launch文件就可直接打开有障碍物的赛道。 + + + +The `nav2_bringup` package is an example bringup system for Nav2 applications. + +This is a very flexible example for nav2 bringup that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific robot system that you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. This is an applied and working demonstration for the default system bringup with many options that can be easily modified. + +Usual robot stacks will have a `_nav` package with config/bringup files and this is that for the general case to base a specific robot system off of. + +Dynamically composed bringup (based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html)) is optional for users. It can be used to compose all Nav2 nodes in a single process instead of launching these nodes separately, which is useful for embedded systems users that need to make optimizations due to harsh resource constraints. Dynamically composed bringup is used by default, but can be disabled by using the launch argument `use_composition:=False`. + +* Some discussions about performance improvement of composed bringup could be found here: https://discourse.ros.org/t/nav2-composition/22175. + +To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. + +Note: +* gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly. +* spawn_entity node could not remap /tf and /tf_static to tf and tf_static in the launch file yet, used only for multi-robot situations. Instead it should be done as remapping argument /tf:=tf /tf_static:=tf_static under ros2 tag in each plugin which publishs transforms in the SDF file. It is essential to differentiate the tf's of the different robot. + +## Launch + +### Multi-robot Simulation + +This is how to launch multi-robot simulation with simple command line. Please see the Nav2 documentation for further augments. + +#### Cloned + +This allows to bring up multiple robots, cloning a single robot N times at different positions in the map. The parameter are loaded from `nav2_multirobot_params_all.yaml` file by default. +The multiple robots that consists of name and initial pose in YAML format will be set on the command-line. The format for each robot is `robot_name={x: 0.0, y: 0.0, yaw: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0}`. + +Please refer to below examples. + +```shell +ros2 launch nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" +``` + +#### Unique + +There are two robots including name and intitial pose are hard-coded in the launch script. Two separated unique robots are required params file (`nav2_multirobot_params_1.yaml`, `nav2_multirobot_params_2.yaml`) for each robot to bring up. + +If you want to bringup more than two robots, you should modify the `unique_multi_tb3_simulation_launch.py` script. + +```shell +ros2 launch nav2_bringup unique_multi_tb3_simulation_launch.py +``` diff --git a/plc_bringup/launch/bringup_launch.py b/plc_bringup/launch/bringup_launch.py new file mode 100644 index 0000000..a29ba11 --- /dev/null +++ b/plc_bringup/launch/bringup_launch.py @@ -0,0 +1,216 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, GroupAction, + IncludeLaunchDescription, SetEnvironmentVariable) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import RewrittenYaml, ReplaceString + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('plc_bringup') + launch_dir = os.path.join(bringup_dir, 'launch') + + # Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') + slam = LaunchConfiguration('slam') + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} + + # Only it applys when `use_namespace` is True. + # '' keyword shall be replaced by 'namespace' launch argument + # in config file 'nav2_multirobot_params.yaml' as a default & example. + # User defined config file should contain '' keyword for the replacements. + params_file = ReplaceString( + source_file=params_file, + replacements={'': ('/', namespace)}, + condition=IfCondition(use_namespace)) + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack') + + declare_slam_cmd = DeclareLaunchArgument( + 'slam', + default_value='False', + description='Whether run a SLAM') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + description='Full path to map yaml file to load') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Whether to use composed bringup') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + # Specify the actions + bringup_cmd_group = GroupAction([ + PushRosNamespace( + condition=IfCondition(use_namespace), + namespace=namespace), + + Node( + condition=IfCondition(use_composition), + name='nav2_container', + package='rclcpp_components', + executable='component_container_isolated', + parameters=[configured_params, {'autostart': autostart}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + output='screen'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), + condition=IfCondition(slam), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'use_respawn': use_respawn, + 'params_file': params_file}.items()), + + # Node( + # package='nav2_map_server', + # executable='map_server', + # name='map_server', + # output='screen', + # respawn=use_respawn, + # respawn_delay=2.0, + # parameters={'map':map_yaml_file}, + # arguments=['--ros-args', '--log-level', log_level], + # remappings=remappings), + # 直接发布 odom 到 map 省去amcl + Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments = ['--x', '0', '--y', '0', '--z', '0', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'map', '--child-frame-id', 'odom']), + + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, + 'localization_launch.py')), + condition=IfCondition(PythonExpression(['not ', slam])), + launch_arguments={'namespace': namespace, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container'}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container'}.items()), + ]) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(bringup_cmd_group) + + return ld + + diff --git a/plc_bringup/launch/cloned_multi_tb3_simulation_launch.py b/plc_bringup/launch/cloned_multi_tb3_simulation_launch.py new file mode 100644 index 0000000..fc1499a --- /dev/null +++ b/plc_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -0,0 +1,174 @@ +# Copyright (c) 2023 LG Electronics. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, + IncludeLaunchDescription, LogInfo) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, TextSubstitution +from nav2_common.launch import ParseMultiRobotPose + + +def generate_launch_description(): + """ + Bring up the multi-robots with given launch arguments. + + Launch arguments consist of robot name(which is namespace) and pose for initialization. + Keep general yaml format for pose information. + ex) robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" + ex) robots:="robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}; + robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}" + """ + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + launch_dir = os.path.join(bringup_dir, 'launch') + + # Simulation settings + world = LaunchConfiguration('world') + simulator = LaunchConfiguration('simulator') + + # On this example all robots are launched with the same settings + map_yaml_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + rviz_config_file = LaunchConfiguration('rviz_config') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + log_settings = LaunchConfiguration('log_settings', default='true') + + # Declare the launch arguments + declare_world_cmd = DeclareLaunchArgument( + 'world', + default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), + description='Full path to world file to load') + + declare_simulator_cmd = DeclareLaunchArgument( + 'simulator', + default_value='gazebo', + description='The simulator to use (gazebo or gzserver)') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map file to load') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_all.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='false', + description='Automatically startup the stacks') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), + description='Full path to the RVIZ config file to use.') + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher') + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + # Start Gazebo with plugin providing the robot spawning service + start_gazebo_cmd = ExecuteProcess( + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], + output='screen') + + robots_list = ParseMultiRobotPose('robots').value() + + # Define commands for launching the navigation instances + bringup_cmd_group = [] + for robot_name in robots_list: + init_pose = robots_list[robot_name] + group = GroupAction([ + LogInfo(msg=['Launching namespace=', robot_name, ' init_pose=', str(init_pose)]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': TextSubstitution(text=robot_name), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, + 'launch', + 'tb3_simulation_launch.py')), + launch_arguments={'namespace': robot_name, + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(init_pose['x'])), + 'y_pose': TextSubstitution(text=str(init_pose['y'])), + 'z_pose': TextSubstitution(text=str(init_pose['z'])), + 'roll': TextSubstitution(text=str(init_pose['roll'])), + 'pitch': TextSubstitution(text=str(init_pose['pitch'])), + 'yaw': TextSubstitution(text=str(init_pose['yaw'])), + 'robot_name':TextSubstitution(text=robot_name), }.items()) + ]) + + bringup_cmd_group.append(group) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + + # Add the actions to start gazebo, robots and simulations + ld.add_action(start_gazebo_cmd) + + ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))])) + + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['map yaml: ', map_yaml_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['params yaml: ', params_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['rviz config file: ', rviz_config_file])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['using robot state pub: ', use_robot_state_pub])) + ld.add_action(LogInfo(condition=IfCondition(log_settings), + msg=['autostart: ', autostart])) + + for cmd in bringup_cmd_group: + ld.add_action(cmd) + + return ld diff --git a/plc_bringup/launch/localization_launch.py b/plc_bringup/launch/localization_launch.py new file mode 100644 index 0000000..16a878f --- /dev/null +++ b/plc_bringup/launch/localization_launch.py @@ -0,0 +1,197 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('plc_bringup') + + namespace = LaunchConfiguration('namespace') + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['map_server', 'amcl'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + description='Full path to map yaml file to load') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + # 直接发布 odom 到 map 省去amcl + # Node( + # package='tf2_ros', + # executable='static_transform_publisher', + # arguments = ['--x', '0', '--y', '0', '--z', '0', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'map', '--child-frame-id', 'odom']), + Node( + package='nav2_amcl', + executable='amcl', + name='amcl', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_localization', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_amcl', + plugin='nav2_amcl::AmclNode', + name='amcl', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_localization', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/plc_bringup/launch/navigation_launch.py b/plc_bringup/launch/navigation_launch.py new file mode 100644 index 0000000..004d353 --- /dev/null +++ b/plc_bringup/launch/navigation_launch.py @@ -0,0 +1,272 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('plc_bringup') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/plc_bringup/launch/rviz_launch.py b/plc_bringup/launch/rviz_launch.py new file mode 100644 index 0000000..29194ac --- /dev/null +++ b/plc_bringup/launch/rviz_launch.py @@ -0,0 +1,109 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import ReplaceString + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + # Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') + rviz_config_file = LaunchConfiguration('rviz_config') + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='navigation', + description=('Top-level namespace. The value will be used to replace the ' + ' keyword on the rviz config file.')) + + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use') + + # Launch rviz + start_rviz_cmd = Node( + condition=UnlessCondition(use_namespace), + package='rviz2', + executable='rviz2', + arguments=['-d', rviz_config_file], + output='screen') + + namespaced_rviz_config_file = ReplaceString( + source_file=rviz_config_file, + replacements={'': ('/', namespace)}) + + start_namespaced_rviz_cmd = Node( + condition=IfCondition(use_namespace), + package='rviz2', + executable='rviz2', + namespace=namespace, + arguments=['-d', namespaced_rviz_config_file], + output='screen', + remappings=[('/map', 'map'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ('/goal_pose', 'goal_pose'), + ('/clicked_point', 'clicked_point'), + ('/initialpose', 'initialpose')]) + + exit_event_handler = RegisterEventHandler( + condition=UnlessCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_rviz_cmd, + on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) + + exit_event_handler_namespaced = RegisterEventHandler( + condition=IfCondition(use_namespace), + event_handler=OnProcessExit( + target_action=start_namespaced_rviz_cmd, + on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_rviz_config_file_cmd) + + # Add any conditioned actions + ld.add_action(start_rviz_cmd) + ld.add_action(start_namespaced_rviz_cmd) + + # Add other nodes and processes we need + ld.add_action(exit_event_handler) + ld.add_action(exit_event_handler_namespaced) + + return ld diff --git a/plc_bringup/launch/slam_launch.py b/plc_bringup/launch/slam_launch.py new file mode 100644 index 0000000..4562ed4 --- /dev/null +++ b/plc_bringup/launch/slam_launch.py @@ -0,0 +1,142 @@ +# Copyright (c) 2020 Samsung Research Russia +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import HasNodeParams, RewrittenYaml + + +def generate_launch_description(): + # Input parameters declaration + namespace = LaunchConfiguration('namespace') + params_file = LaunchConfiguration('params_file') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + # Variables + lifecycle_nodes = ['map_saver'] + + # Getting directories and launch-files + bringup_dir = get_package_share_directory('plc_bringup') + slam_toolbox_dir = get_package_share_directory('slam_toolbox') + slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'online_sync_launch.py') + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='True', + description='Automatically startup the nav2 stack') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + # Nodes launching commands + + start_map_saver_server_cmd = Node( + package='nav2_map_server', + executable='map_saver_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + arguments=['--ros-args', '--log-level', log_level], + parameters=[configured_params]) + + start_lifecycle_manager_cmd = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_slam', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + + # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' + # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load + # the default file + has_slam_toolbox_params = HasNodeParams(source_file=params_file, + node_name='slam_toolbox') + + start_slam_toolbox_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time}.items(), + condition=UnlessCondition(has_slam_toolbox_params)) + + start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time, + 'slam_params_file': params_file}.items(), + condition=IfCondition(has_slam_toolbox_params)) + + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Running Map Saver Server + ld.add_action(start_map_saver_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) + + # Running SLAM Toolbox (Only one of them will be run) + ld.add_action(start_slam_toolbox_cmd) + ld.add_action(start_slam_toolbox_cmd_with_params) + + return ld diff --git a/plc_bringup/launch/tb3_simulation_launch.py b/plc_bringup/launch/tb3_simulation_launch.py new file mode 100644 index 0000000..9154cf9 --- /dev/null +++ b/plc_bringup/launch/tb3_simulation_launch.py @@ -0,0 +1,254 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""This is all-in-one launch script intended for use by nav2 developers.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('plc_bringup') + launch_dir = os.path.join(bringup_dir, 'launch') + + # Create the launch configuration variables + slam = LaunchConfiguration('slam') + namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + + # Launch configuration variables specific to simulation + rviz_config_file = LaunchConfiguration('rviz_config_file') + use_simulator = LaunchConfiguration('use_simulator') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + headless = LaunchConfiguration('headless') + world = LaunchConfiguration('world') + pose = {'x': LaunchConfiguration('x_pose', default='-0'), + 'y': LaunchConfiguration('y_pose', default='-5.296'), + 'z': LaunchConfiguration('z_pose', default='0.01'), + 'R': LaunchConfiguration('roll', default='0.00'), + 'P': LaunchConfiguration('pitch', default='0.00'), + 'Y': LaunchConfiguration('yaw', default='0.00')} + robot_name = LaunchConfiguration('robot_name') + robot_sdf = LaunchConfiguration('robot_sdf') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack') + + declare_slam_cmd = DeclareLaunchArgument( + 'slam', + default_value='False', + description='Whether run a SLAM') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join( + bringup_dir, 'maps', 'xunfei.yaml'), + description='Full path to map file to load') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Whether to use composed bringup') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config_file', + default_value=os.path.join( + bringup_dir, 'rviz', 'nav2_default_view.rviz'), + description='Full path to the RVIZ config file to use') + + declare_use_simulator_cmd = DeclareLaunchArgument( + 'use_simulator', + default_value='True', + description='Whether to start the simulator') + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher') + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + declare_simulator_cmd = DeclareLaunchArgument( + 'headless', + default_value='False', + description='Whether to execute gzclient)') + + declare_world_cmd = DeclareLaunchArgument( + 'world', + # TODO(orduno) Switch back once ROS argument passing has been fixed upstream + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91 + # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), + # worlds/turtlebot3_worlds/waffle.model') + default_value=os.path.join(bringup_dir, 'worlds', 'race_with_cone2.world'), + description='Full path to world model file to load') + + declare_robot_name_cmd = DeclareLaunchArgument( + 'robot_name', + default_value='turtlebot3_waffle', + description='name of the robot') + + declare_robot_sdf_cmd = DeclareLaunchArgument( + 'robot_sdf', + default_value=os.path.join(bringup_dir, 'urdf', 'myrobot'), + # default_value=os.path.join(bringup_dir, 'world', 'waffle.model'), + description='Full path to robot sdf file to spawn the robot in gazebo') + + # Specify the actions + start_gazebo_server_cmd = ExecuteProcess( + condition=IfCondition(use_simulator), + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], + cwd=[launch_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + condition=IfCondition(PythonExpression( + [use_simulator, ' and not ', headless])), + cmd=['gzclient'], + cwd=[launch_dir], output='screen') + + urdf = os.path.join(bringup_dir, 'urdf', 'myrobot.urdf') + # urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + start_robot_state_publisher_cmd = Node( + condition=IfCondition(use_robot_state_pub), + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + namespace=namespace, + output='screen', + parameters=[{'use_sim_time': use_sim_time, + 'robot_description': robot_description}], + remappings=remappings) + + start_gazebo_spawner_cmd = Node( + package='gazebo_ros', + executable='spawn_entity.py', + output='screen', + arguments=[ + '-entity', robot_name, + '-file', robot_sdf, + '-robot_namespace', namespace, + '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], + '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']]) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={'namespace': namespace, + 'use_namespace': use_namespace, + 'rviz_config': rviz_config_file}.items()) + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={'namespace': namespace, + 'use_namespace': use_namespace, + 'slam': slam, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn}.items()) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_simulator_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_robot_name_cmd) + ld.add_action(declare_robot_sdf_cmd) + ld.add_action(declare_use_respawn_cmd) + + # Add any conditioned actions + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + + return ld diff --git a/plc_bringup/launch/unique_multi_tb3_simulation_launch.py b/plc_bringup/launch/unique_multi_tb3_simulation_launch.py new file mode 100644 index 0000000..d91c5ce --- /dev/null +++ b/plc_bringup/launch/unique_multi_tb3_simulation_launch.py @@ -0,0 +1,190 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Example for spawning multiple robots in Gazebo. + +This is an example on how to create a launch file for spawning multiple robots into Gazebo +and launch multiple instances of the navigation stack, each controlling one robot. +The robots co-exist on a shared environment and are controlled by independent nav stacks. +""" + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, GroupAction, + IncludeLaunchDescription, LogInfo) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, TextSubstitution + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + launch_dir = os.path.join(bringup_dir, 'launch') + + # Names and poses of the robots + robots = [ + {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01, + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}, + {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01, + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}] + + # Simulation settings + world = LaunchConfiguration('world') + simulator = LaunchConfiguration('simulator') + + # On this example all robots are launched with the same settings + map_yaml_file = LaunchConfiguration('map') + + autostart = LaunchConfiguration('autostart') + rviz_config_file = LaunchConfiguration('rviz_config') + use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') + use_rviz = LaunchConfiguration('use_rviz') + log_settings = LaunchConfiguration('log_settings', default='true') + + # Declare the launch arguments + declare_world_cmd = DeclareLaunchArgument( + 'world', + default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), + description='Full path to world file to load') + + declare_simulator_cmd = DeclareLaunchArgument( + 'simulator', + default_value='gazebo', + description='The simulator to use (gazebo or gzserver)') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map file to load') + + declare_robot1_params_file_cmd = DeclareLaunchArgument( + 'robot1_params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_1.yaml'), + description='Full path to the ROS2 parameters file to use for robot1 launched nodes') + + declare_robot2_params_file_cmd = DeclareLaunchArgument( + 'robot2_params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_2.yaml'), + description='Full path to the ROS2 parameters file to use for robot2 launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='false', + description='Automatically startup the stacks') + + declare_rviz_config_file_cmd = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'), + description='Full path to the RVIZ config file to use.') + + declare_use_robot_state_pub_cmd = DeclareLaunchArgument( + 'use_robot_state_pub', + default_value='True', + description='Whether to start the robot state publisher') + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RVIZ') + + # Start Gazebo with plugin providing the robot spawning service + start_gazebo_cmd = ExecuteProcess( + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], + output='screen') + + # Define commands for launching the navigation instances + nav_instances_cmds = [] + for robot in robots: + params_file = LaunchConfiguration(f"{robot['name']}_params_file") + + group = GroupAction([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), + condition=IfCondition(use_rviz), + launch_arguments={ + 'namespace': TextSubstitution(text=robot['name']), + 'use_namespace': 'True', + 'rviz_config': rviz_config_file}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, + 'launch', + 'tb3_simulation_launch.py')), + launch_arguments={'namespace': robot['name'], + 'use_namespace': 'True', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'autostart': autostart, + 'use_rviz': 'False', + 'use_simulator': 'False', + 'headless': 'False', + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(robot['x_pose'])), + 'y_pose': TextSubstitution(text=str(robot['y_pose'])), + 'z_pose': TextSubstitution(text=str(robot['z_pose'])), + 'roll': TextSubstitution(text=str(robot['roll'])), + 'pitch': TextSubstitution(text=str(robot['pitch'])), + 'yaw': TextSubstitution(text=str(robot['yaw'])), + 'robot_name':TextSubstitution(text=robot['name']), }.items()), + + LogInfo( + condition=IfCondition(log_settings), + msg=['Launching ', robot['name']]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' map yaml: ', map_yaml_file]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' params yaml: ', params_file]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' rviz config file: ', rviz_config_file]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' using robot state pub: ', use_robot_state_pub]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' autostart: ', autostart]) + ]) + + nav_instances_cmds.append(group) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_robot1_params_file_cmd) + ld.add_action(declare_robot2_params_file_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_robot_state_pub_cmd) + + # Add the actions to start gazebo, robots and simulations + ld.add_action(start_gazebo_cmd) + + for simulation_instance_cmd in nav_instances_cmds: + ld.add_action(simulation_instance_cmd) + + return ld diff --git a/plc_bringup/maps/turtlebot3_world.pgm b/plc_bringup/maps/turtlebot3_world.pgm new file mode 100644 index 0000000..10700a1 --- /dev/null +++ b/plc_bringup/maps/turtlebot3_world.pgm @@ -0,0 +1,5 @@ +P5 +# CREATOR: Map_generator.cpp 0.050 m/pix +384 384 +255 +�� \ No newline at end of file diff --git a/plc_bringup/maps/turtlebot3_world.yaml b/plc_bringup/maps/turtlebot3_world.yaml new file mode 100644 index 0000000..51d4387 --- /dev/null +++ b/plc_bringup/maps/turtlebot3_world.yaml @@ -0,0 +1,6 @@ +image: turtlebot3_world.pgm +resolution: 0.050000 +origin: [-10.000000, -10.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/plc_bringup/maps/xunfei.pgm b/plc_bringup/maps/xunfei.pgm new file mode 100644 index 0000000..fd52bd5 Binary files /dev/null and b/plc_bringup/maps/xunfei.pgm differ diff --git a/plc_bringup/maps/xunfei.yaml b/plc_bringup/maps/xunfei.yaml new file mode 100644 index 0000000..332afb4 --- /dev/null +++ b/plc_bringup/maps/xunfei.yaml @@ -0,0 +1,7 @@ +image: xunfei.pgm +# mode: trinary +resolution: 0.05 +origin: [-0.557, -6.82, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.1 diff --git a/plc_bringup/maps/xunfei_backup.pgm b/plc_bringup/maps/xunfei_backup.pgm new file mode 100644 index 0000000..6679157 Binary files /dev/null and b/plc_bringup/maps/xunfei_backup.pgm differ diff --git a/plc_bringup/meshes/bot.dae b/plc_bringup/meshes/bot.dae new file mode 100644 index 0000000..1ef04f6 --- /dev/null +++ b/plc_bringup/meshes/bot.dae @@ -0,0 +1,123 @@ + + + + + Google SketchUp 8.0.3161 + + 2013-03-12T18:37:32Z + 2013-03-12T18:37:32Z + + Z_UP + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.0000000 -45.7519685 7.1023622 9.2094319 -27.6589793 7.1023622 0.0000000 0.0000000 7.1023622 27.3385827 -45.7519685 7.1023622 19.5952586 -27.6589793 7.1023622 19.5952586 -15.2455935 7.1023622 9.2094319 -15.2455935 7.1023622 27.3385827 0.0000000 7.1023622 0.0000000 -45.7519685 7.1023622 0.0000000 -42.4667573 3.5912116 0.0000000 -45.7519685 0.0000000 0.0000000 -41.4149158 4.3983180 0.0000000 -40.1900204 4.9056863 0.0000000 -38.8755457 5.0787402 0.0000000 0.0000000 7.1023622 0.0000000 -43.2738637 2.5393701 0.0000000 -43.7812320 1.3144747 0.0000000 -43.9542859 0.0000000 0.0000000 -37.5610710 4.9056863 0.0000000 -36.3361756 4.3983180 0.0000000 -35.2843341 3.5912116 0.0000000 -34.4772277 2.5393701 0.0000000 -33.9698594 1.3144747 0.0000000 -33.7968056 -0.0000000 0.0000000 -7.4224696 4.8068120 0.0000000 -8.6226772 4.3096697 0.0000000 -9.6533188 3.5188306 0.0000000 -10.4441579 2.4881890 0.0000000 -10.9413002 1.2879814 0.0000000 -11.1108661 0.0000000 0.0000000 -6.1344882 4.9763780 0.0000000 -4.8465068 4.8068120 0.0000000 -3.6462992 4.3096697 0.0000000 -2.6156576 3.5188306 0.0000000 -1.8248185 2.4881890 0.0000000 -1.3276762 1.2879814 0.0000000 -1.1581102 -0.0000000 0.0000000 0.0000000 0.0000000 27.3385827 -45.7519685 7.1023622 0.0000000 -45.7519685 0.0000000 27.3385827 -45.7519685 0.0000000 0.0000000 -45.7519685 7.1023622 27.3385827 -43.9542859 -0.0000000 27.3385827 -45.7519685 7.1023622 27.3385827 -45.7519685 0.0000000 27.3385827 -43.7812320 1.3144747 27.3385827 -43.2738637 2.5393701 27.3385827 -42.4667573 3.5912116 27.3385827 -41.4149158 4.3983180 27.3385827 -40.1900204 4.9056863 27.3385827 -38.8755457 5.0787402 27.3385827 0.0000000 7.1023622 27.3385827 -37.5610710 4.9056863 27.3385827 -36.3361756 4.3983180 27.3385827 -35.2843341 3.5912116 27.3385827 -34.4772277 2.5393701 27.3385827 -33.9698594 1.3144747 27.3385827 -33.7968056 0.0000000 27.3385827 -7.4224696 4.8068120 27.3385827 -8.6226772 4.3096697 27.3385827 -9.6533188 3.5188306 27.3385827 -10.4441579 2.4881890 27.3385827 -10.9413002 1.2879814 27.3385827 -11.1108661 -0.0000000 27.3385827 -6.1344882 4.9763780 27.3385827 -4.8465068 4.8068120 27.3385827 -3.6462992 4.3096697 27.3385827 -2.6156576 3.5188306 27.3385827 -1.8248185 2.4881890 27.3385827 -1.3276762 1.2879814 27.3385827 -1.1581102 0.0000000 27.3385827 0.0000000 0.0000000 0.0000000 0.0000000 7.1023622 27.3385827 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 27.3385827 0.0000000 7.1023622 30.0078740 -1.1581102 0.0000000 27.3385827 -11.1108661 -0.0000000 27.3385827 -1.1581102 0.0000000 30.0078740 -11.1108661 -0.0000000 27.3385827 0.0000000 0.0000000 0.0000000 -1.1581102 -0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 -11.1108661 0.0000000 0.0000000 -33.7968056 -0.0000000 0.0000000 -43.9542859 0.0000000 0.0000000 -45.7519685 0.0000000 27.3385827 -45.7519685 0.0000000 27.3385827 -33.7968056 0.0000000 27.3385827 -43.9542859 -0.0000000 -3.6456693 -1.3276762 1.2879814 0.0000000 -1.1581102 -0.0000000 -3.6456693 -1.1581102 -0.0000000 0.0000000 -1.3276762 1.2879814 -3.6456693 -1.8248185 2.4881890 0.0000000 -1.8248185 2.4881890 -3.6456693 -2.6156576 3.5188306 0.0000000 -2.6156576 3.5188306 0.0000000 -3.6462992 4.3096697 -3.6456693 -3.6462992 4.3096697 0.0000000 -4.8465068 4.8068120 -3.6456693 -4.8465068 4.8068120 0.0000000 -6.1344882 4.9763780 -3.6456693 -6.1344882 4.9763780 0.0000000 -7.4224696 4.8068120 -3.6456693 -7.4224696 4.8068120 0.0000000 -8.6226772 4.3096697 -3.6456693 -8.6226772 4.3096697 0.0000000 -9.6533188 3.5188306 -3.6456693 -9.6533188 3.5188306 -3.6456693 -10.4441579 2.4881890 0.0000000 -10.4441579 2.4881890 -3.6456693 -10.9413002 1.2879814 0.0000000 -10.9413002 1.2879814 -3.6456693 -11.1108661 0.0000000 0.0000000 -11.1108661 0.0000000 -3.6456693 -33.9698594 1.3144747 0.0000000 -33.7968056 -0.0000000 -3.6456693 -33.7968056 -0.0000000 0.0000000 -33.9698594 1.3144747 -3.6456693 -34.4772277 2.5393701 0.0000000 -34.4772277 2.5393701 -3.6456693 -35.2843341 3.5912116 0.0000000 -35.2843341 3.5912116 0.0000000 -36.3361756 4.3983180 -3.6456693 -36.3361756 4.3983180 0.0000000 -37.5610710 4.9056863 -3.6456693 -37.5610710 4.9056863 0.0000000 -38.8755457 5.0787402 -3.6456693 -38.8755457 5.0787402 0.0000000 -40.1900204 4.9056863 -3.6456693 -40.1900204 4.9056863 0.0000000 -41.4149158 4.3983180 -3.6456693 -41.4149158 4.3983180 0.0000000 -42.4667573 3.5912116 -3.6456693 -42.4667573 3.5912116 -3.6456693 -43.2738637 2.5393701 0.0000000 -43.2738637 2.5393701 -3.6456693 -43.7812320 1.3144747 0.0000000 -43.7812320 1.3144747 -3.6456693 -43.9542859 0.0000000 0.0000000 -43.9542859 0.0000000 30.0078740 -10.9413002 1.2879814 27.3385827 -11.1108661 -0.0000000 30.0078740 -11.1108661 -0.0000000 27.3385827 -10.9413002 1.2879814 30.0078740 -10.4441579 2.4881890 27.3385827 -10.4441579 2.4881890 30.0078740 -9.6533188 3.5188306 27.3385827 -9.6533188 3.5188306 27.3385827 -8.6226772 4.3096697 30.0078740 -8.6226772 4.3096697 27.3385827 -7.4224696 4.8068120 30.0078740 -7.4224696 4.8068120 27.3385827 -6.1344882 4.9763780 30.0078740 -6.1344882 4.9763780 27.3385827 -4.8465068 4.8068120 30.0078740 -4.8465068 4.8068120 27.3385827 -3.6462992 4.3096697 30.0078740 -3.6462992 4.3096697 27.3385827 -2.6156576 3.5188306 30.0078740 -2.6156576 3.5188306 30.0078740 -1.8248185 2.4881890 27.3385827 -1.8248185 2.4881890 30.0078740 -1.3276762 1.2879814 27.3385827 -1.3276762 1.2879814 30.0078740 -1.1581102 0.0000000 27.3385827 -1.1581102 0.0000000 30.0078740 -43.7812320 1.3144747 27.3385827 -43.9542859 -0.0000000 30.0078740 -43.9542859 -0.0000000 27.3385827 -43.7812320 1.3144747 30.0078740 -43.2738637 2.5393701 27.3385827 -43.2738637 2.5393701 30.0078740 -42.4667573 3.5912116 27.3385827 -42.4667573 3.5912116 27.3385827 -41.4149158 4.3983180 30.0078740 -41.4149158 4.3983180 27.3385827 -40.1900204 4.9056863 30.0078740 -40.1900204 4.9056863 27.3385827 -38.8755457 5.0787402 30.0078740 -38.8755457 5.0787402 27.3385827 -37.5610710 4.9056863 30.0078740 -37.5610710 4.9056863 27.3385827 -36.3361756 4.3983180 30.0078740 -36.3361756 4.3983180 27.3385827 -35.2843341 3.5912116 30.0078740 -35.2843341 3.5912116 30.0078740 -34.4772277 2.5393701 27.3385827 -34.4772277 2.5393701 30.0078740 -33.9698594 1.3144747 27.3385827 -33.9698594 1.3144747 30.0078740 -33.7968056 0.0000000 27.3385827 -33.7968056 0.0000000 27.3385827 -33.7968056 0.0000000 27.3385827 -43.7812320 -1.3144747 27.3385827 -43.9542859 -0.0000000 27.3385827 -43.2738637 -2.5393701 27.3385827 -42.4667573 -3.5912116 27.3385827 -41.4149158 -4.3983180 27.3385827 -40.1900204 -4.9056863 27.3385827 -38.8755457 -5.0787402 27.3385827 -37.5610710 -4.9056863 27.3385827 -36.3361756 -4.3983180 27.3385827 -35.2843341 -3.5912116 27.3385827 -34.4772277 -2.5393701 27.3385827 -33.9698594 -1.3144747 30.0078740 -33.7968056 0.0000000 27.3385827 -43.9542859 -0.0000000 27.3385827 -33.7968056 0.0000000 30.0078740 -43.9542859 -0.0000000 0.0000000 -43.7812320 -1.3144747 0.0000000 -33.7968056 -0.0000000 0.0000000 -43.9542859 0.0000000 0.0000000 -43.2738637 -2.5393701 0.0000000 -42.4667573 -3.5912116 0.0000000 -41.4149158 -4.3983180 0.0000000 -40.1900204 -4.9056863 0.0000000 -38.8755457 -5.0787402 0.0000000 -37.5610710 -4.9056863 0.0000000 -36.3361756 -4.3983180 0.0000000 -35.2843341 -3.5912116 0.0000000 -34.4772277 -2.5393701 0.0000000 -33.9698594 -1.3144747 0.0000000 -33.7968056 -0.0000000 -3.6456693 -43.9542859 0.0000000 -3.6456693 -33.7968056 -0.0000000 0.0000000 -43.9542859 0.0000000 0.0000000 -10.9413002 -1.2879814 0.0000000 -1.1581102 -0.0000000 0.0000000 -11.1108661 0.0000000 0.0000000 -10.4441579 -2.4881890 0.0000000 -9.6533188 -3.5188306 0.0000000 -8.6226772 -4.3096697 0.0000000 -7.4224696 -4.8068120 0.0000000 -6.1344882 -4.9763780 0.0000000 -4.8465068 -4.8068120 0.0000000 -3.6462992 -4.3096697 0.0000000 -2.6156576 -3.5188306 0.0000000 -1.8248185 -2.4881890 0.0000000 -1.3276762 -1.2879814 0.0000000 -11.1108661 0.0000000 -3.6456693 -1.1581102 -0.0000000 -3.6456693 -11.1108661 0.0000000 0.0000000 -1.1581102 -0.0000000 27.3385827 -1.1581102 0.0000000 30.0078740 -1.3276762 -1.2879814 27.3385827 -1.3276762 -1.2879814 30.0078740 -1.1581102 0.0000000 30.0078740 -1.1581102 0.0000000 30.0078740 -10.9413002 1.2879814 30.0078740 -11.1108661 -0.0000000 30.0078740 -10.4441579 2.4881890 30.0078740 -9.6533188 3.5188306 30.0078740 -8.6226772 4.3096697 30.0078740 -7.4224696 4.8068120 30.0078740 -6.1344882 4.9763780 30.0078740 -4.8465068 4.8068120 30.0078740 -3.6462992 4.3096697 30.0078740 -2.6156576 3.5188306 30.0078740 -1.8248185 2.4881890 30.0078740 -1.3276762 1.2879814 30.0078740 -10.9413002 -1.2879814 30.0078740 -1.1581102 0.0000000 30.0078740 -11.1108661 -0.0000000 30.0078740 -10.4441579 -2.4881890 30.0078740 -9.6533188 -3.5188306 30.0078740 -8.6226772 -4.3096697 30.0078740 -7.4224696 -4.8068120 30.0078740 -6.1344882 -4.9763780 30.0078740 -4.8465068 -4.8068120 30.0078740 -3.6462992 -4.3096697 30.0078740 -2.6156576 -3.5188306 30.0078740 -1.8248185 -2.4881890 30.0078740 -1.3276762 -1.2879814 30.0078740 -11.1108661 -0.0000000 27.3385827 -10.9413002 -1.2879814 30.0078740 -10.9413002 -1.2879814 27.3385827 -11.1108661 -0.0000000 -3.6456693 -1.1581102 -0.0000000 0.0000000 -1.3276762 -1.2879814 -3.6456693 -1.3276762 -1.2879814 0.0000000 -1.1581102 -0.0000000 -3.6456693 -10.9413002 1.2879814 -3.6456693 -1.1581102 -0.0000000 -3.6456693 -11.1108661 0.0000000 -3.6456693 -10.4441579 2.4881890 -3.6456693 -9.6533188 3.5188306 -3.6456693 -8.6226772 4.3096697 -3.6456693 -7.4224696 4.8068120 -3.6456693 -6.1344882 4.9763780 -3.6456693 -4.8465068 4.8068120 -3.6456693 -3.6462992 4.3096697 -3.6456693 -2.6156576 3.5188306 -3.6456693 -1.8248185 2.4881890 -3.6456693 -1.3276762 1.2879814 0.0000000 -11.1108661 0.0000000 -3.6456693 -10.9413002 -1.2879814 0.0000000 -10.9413002 -1.2879814 -3.6456693 -11.1108661 0.0000000 -3.6456693 -33.7968056 -0.0000000 0.0000000 -33.9698594 -1.3144747 -3.6456693 -33.9698594 -1.3144747 0.0000000 -33.7968056 -0.0000000 -3.6456693 -43.7812320 1.3144747 -3.6456693 -33.7968056 -0.0000000 -3.6456693 -43.9542859 0.0000000 -3.6456693 -43.2738637 2.5393701 -3.6456693 -42.4667573 3.5912116 -3.6456693 -41.4149158 4.3983180 -3.6456693 -40.1900204 4.9056863 -3.6456693 -38.8755457 5.0787402 -3.6456693 -37.5610710 4.9056863 -3.6456693 -36.3361756 4.3983180 -3.6456693 -35.2843341 3.5912116 -3.6456693 -34.4772277 2.5393701 -3.6456693 -33.9698594 1.3144747 0.0000000 -43.9542859 0.0000000 -3.6456693 -43.7812320 -1.3144747 0.0000000 -43.7812320 -1.3144747 -3.6456693 -43.9542859 0.0000000 30.0078740 -43.9542859 -0.0000000 27.3385827 -43.7812320 -1.3144747 30.0078740 -43.7812320 -1.3144747 27.3385827 -43.9542859 -0.0000000 30.0078740 -33.7968056 0.0000000 30.0078740 -43.7812320 1.3144747 30.0078740 -43.9542859 -0.0000000 30.0078740 -43.2738637 2.5393701 30.0078740 -42.4667573 3.5912116 30.0078740 -41.4149158 4.3983180 30.0078740 -40.1900204 4.9056863 30.0078740 -38.8755457 5.0787402 30.0078740 -37.5610710 4.9056863 30.0078740 -36.3361756 4.3983180 30.0078740 -35.2843341 3.5912116 30.0078740 -34.4772277 2.5393701 30.0078740 -33.9698594 1.3144747 27.3385827 -33.7968056 0.0000000 30.0078740 -33.9698594 -1.3144747 27.3385827 -33.9698594 -1.3144747 30.0078740 -33.7968056 0.0000000 30.0078740 -34.4772277 -2.5393701 27.3385827 -34.4772277 -2.5393701 30.0078740 -35.2843341 -3.5912116 27.3385827 -35.2843341 -3.5912116 27.3385827 -36.3361756 -4.3983180 30.0078740 -36.3361756 -4.3983180 27.3385827 -37.5610710 -4.9056863 30.0078740 -37.5610710 -4.9056863 27.3385827 -38.8755457 -5.0787402 30.0078740 -38.8755457 -5.0787402 27.3385827 -40.1900204 -4.9056863 30.0078740 -40.1900204 -4.9056863 27.3385827 -41.4149158 -4.3983180 30.0078740 -41.4149158 -4.3983180 27.3385827 -42.4667573 -3.5912116 30.0078740 -42.4667573 -3.5912116 30.0078740 -43.2738637 -2.5393701 27.3385827 -43.2738637 -2.5393701 30.0078740 -43.7812320 -1.3144747 30.0078740 -43.2738637 -2.5393701 30.0078740 -42.4667573 -3.5912116 30.0078740 -41.4149158 -4.3983180 30.0078740 -40.1900204 -4.9056863 30.0078740 -38.8755457 -5.0787402 30.0078740 -37.5610710 -4.9056863 30.0078740 -36.3361756 -4.3983180 30.0078740 -35.2843341 -3.5912116 30.0078740 -34.4772277 -2.5393701 30.0078740 -33.9698594 -1.3144747 -3.6456693 -43.2738637 -2.5393701 0.0000000 -43.2738637 -2.5393701 -3.6456693 -42.4667573 -3.5912116 0.0000000 -42.4667573 -3.5912116 0.0000000 -41.4149158 -4.3983180 -3.6456693 -41.4149158 -4.3983180 0.0000000 -40.1900204 -4.9056863 -3.6456693 -40.1900204 -4.9056863 0.0000000 -38.8755457 -5.0787402 -3.6456693 -38.8755457 -5.0787402 0.0000000 -37.5610710 -4.9056863 -3.6456693 -37.5610710 -4.9056863 0.0000000 -36.3361756 -4.3983180 -3.6456693 -36.3361756 -4.3983180 0.0000000 -35.2843341 -3.5912116 -3.6456693 -35.2843341 -3.5912116 -3.6456693 -34.4772277 -2.5393701 0.0000000 -34.4772277 -2.5393701 -3.6456693 -43.7812320 -1.3144747 -3.6456693 -43.2738637 -2.5393701 -3.6456693 -42.4667573 -3.5912116 -3.6456693 -41.4149158 -4.3983180 -3.6456693 -40.1900204 -4.9056863 -3.6456693 -38.8755457 -5.0787402 -3.6456693 -37.5610710 -4.9056863 -3.6456693 -36.3361756 -4.3983180 -3.6456693 -35.2843341 -3.5912116 -3.6456693 -34.4772277 -2.5393701 -3.6456693 -33.9698594 -1.3144747 -3.6456693 -10.4441579 -2.4881890 0.0000000 -10.4441579 -2.4881890 -3.6456693 -9.6533188 -3.5188306 0.0000000 -9.6533188 -3.5188306 0.0000000 -8.6226772 -4.3096697 -3.6456693 -8.6226772 -4.3096697 0.0000000 -7.4224696 -4.8068120 -3.6456693 -7.4224696 -4.8068120 0.0000000 -6.1344882 -4.9763780 -3.6456693 -6.1344882 -4.9763780 0.0000000 -4.8465068 -4.8068120 -3.6456693 -4.8465068 -4.8068120 0.0000000 -3.6462992 -4.3096697 -3.6456693 -3.6462992 -4.3096697 0.0000000 -2.6156576 -3.5188306 -3.6456693 -2.6156576 -3.5188306 -3.6456693 -1.8248185 -2.4881890 0.0000000 -1.8248185 -2.4881890 -3.6456693 -10.9413002 -1.2879814 -3.6456693 -10.4441579 -2.4881890 -3.6456693 -9.6533188 -3.5188306 -3.6456693 -8.6226772 -4.3096697 -3.6456693 -7.4224696 -4.8068120 -3.6456693 -6.1344882 -4.9763780 -3.6456693 -4.8465068 -4.8068120 -3.6456693 -3.6462992 -4.3096697 -3.6456693 -2.6156576 -3.5188306 -3.6456693 -1.8248185 -2.4881890 -3.6456693 -1.3276762 -1.2879814 27.3385827 -1.1581102 0.0000000 27.3385827 -10.9413002 -1.2879814 27.3385827 -11.1108661 -0.0000000 27.3385827 -10.4441579 -2.4881890 27.3385827 -9.6533188 -3.5188306 27.3385827 -8.6226772 -4.3096697 27.3385827 -7.4224696 -4.8068120 27.3385827 -6.1344882 -4.9763780 27.3385827 -4.8465068 -4.8068120 27.3385827 -3.6462992 -4.3096697 27.3385827 -2.6156576 -3.5188306 27.3385827 -1.8248185 -2.4881890 27.3385827 -1.3276762 -1.2879814 30.0078740 -1.8248185 -2.4881890 27.3385827 -1.8248185 -2.4881890 27.3385827 -10.4441579 -2.4881890 30.0078740 -10.4441579 -2.4881890 27.3385827 -9.6533188 -3.5188306 30.0078740 -9.6533188 -3.5188306 30.0078740 -8.6226772 -4.3096697 27.3385827 -8.6226772 -4.3096697 30.0078740 -7.4224696 -4.8068120 27.3385827 -7.4224696 -4.8068120 30.0078740 -6.1344882 -4.9763780 27.3385827 -6.1344882 -4.9763780 30.0078740 -4.8465068 -4.8068120 27.3385827 -4.8465068 -4.8068120 30.0078740 -3.6462992 -4.3096697 27.3385827 -3.6462992 -4.3096697 30.0078740 -2.6156576 -3.5188306 27.3385827 -2.6156576 -3.5188306 + + + + + + + + + + 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -0.0000000 -1.0000000 -0.0000000 -0.0000000 -1.0000000 -0.0000000 -0.0000000 -1.0000000 -0.0000000 -0.0000000 -1.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 -0.0000000 1.0000000 0.0000000 -0.0000000 1.0000000 0.0000000 -0.0000000 1.0000000 0.0000000 -0.0000000 1.0000000 0.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.9659258 0.2588190 -0.0000000 0.9914449 0.1305262 -0.0000000 0.9914449 0.1305262 0.0000000 0.9659258 0.2588190 0.0000000 0.8660254 0.5000000 0.0000000 0.8660254 0.5000000 0.0000000 0.7071068 0.7071068 0.0000000 0.7071068 0.7071068 0.0000000 0.5000000 0.8660254 0.0000000 0.5000000 0.8660254 0.0000000 0.2588190 0.9659258 0.0000000 0.2588190 0.9659258 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 -0.2588190 0.9659258 0.0000000 -0.2588190 0.9659258 0.0000000 -0.5000000 0.8660254 0.0000000 -0.5000000 0.8660254 0.0000000 -0.7071068 0.7071068 0.0000000 -0.7071068 0.7071068 0.0000000 -0.8660254 0.5000000 0.0000000 -0.8660254 0.5000000 0.0000000 -0.9659258 0.2588190 0.0000000 -0.9659258 0.2588190 0.0000000 -0.9914449 0.1305262 0.0000000 -0.9914449 0.1305262 0.0000000 0.9659258 0.2588190 -0.0000000 0.9914449 0.1305262 -0.0000000 0.9914449 0.1305262 0.0000000 0.9659258 0.2588190 0.0000000 0.8660254 0.5000000 0.0000000 0.8660254 0.5000000 0.0000000 0.7071068 0.7071068 0.0000000 0.7071068 0.7071068 0.0000000 0.5000000 0.8660254 0.0000000 0.5000000 0.8660254 0.0000000 0.2588190 0.9659258 0.0000000 0.2588190 0.9659258 0.0000000 -0.0000000 1.0000000 0.0000000 -0.0000000 1.0000000 0.0000000 -0.2588190 0.9659258 0.0000000 -0.2588190 0.9659258 0.0000000 -0.5000000 0.8660254 0.0000000 -0.5000000 0.8660254 0.0000000 -0.7071068 0.7071068 0.0000000 -0.7071068 0.7071068 0.0000000 -0.8660254 0.5000000 0.0000000 -0.8660254 0.5000000 0.0000000 -0.9659258 0.2588190 0.0000000 -0.9659258 0.2588190 0.0000000 -0.9914449 0.1305262 0.0000000 -0.9914449 0.1305262 0.0000000 -0.9659258 0.2588190 0.0000000 -0.9914449 0.1305262 0.0000000 -0.9914449 0.1305262 0.0000000 -0.9659258 0.2588190 0.0000000 -0.8660254 0.5000000 0.0000000 -0.8660254 0.5000000 0.0000000 -0.7071068 0.7071068 0.0000000 -0.7071068 0.7071068 0.0000000 -0.5000000 0.8660254 0.0000000 -0.5000000 0.8660254 0.0000000 -0.2588190 0.9659258 0.0000000 -0.2588190 0.9659258 0.0000000 -0.0000000 1.0000000 0.0000000 -0.0000000 1.0000000 0.0000000 0.2588190 0.9659258 0.0000000 0.2588190 0.9659258 0.0000000 0.5000000 0.8660254 0.0000000 0.5000000 0.8660254 0.0000000 0.7071068 0.7071068 0.0000000 0.7071068 0.7071068 0.0000000 0.8660254 0.5000000 0.0000000 0.8660254 0.5000000 0.0000000 0.9659258 0.2588190 0.0000000 0.9659258 0.2588190 -0.0000000 0.9914449 0.1305262 -0.0000000 0.9914449 0.1305262 -0.0000000 -0.9659258 0.2588190 -0.0000000 -0.9914449 0.1305262 -0.0000000 -0.9914449 0.1305262 -0.0000000 -0.9659258 0.2588190 0.0000000 -0.8660254 0.5000000 0.0000000 -0.8660254 0.5000000 0.0000000 -0.7071068 0.7071068 0.0000000 -0.7071068 0.7071068 0.0000000 -0.5000000 0.8660254 0.0000000 -0.5000000 0.8660254 0.0000000 -0.2588190 0.9659258 0.0000000 -0.2588190 0.9659258 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.2588190 0.9659258 0.0000000 0.2588190 0.9659258 0.0000000 0.5000000 0.8660254 0.0000000 0.5000000 0.8660254 0.0000000 0.7071068 0.7071068 0.0000000 0.7071068 0.7071068 0.0000000 0.8660254 0.5000000 0.0000000 0.8660254 0.5000000 0.0000000 0.9659258 0.2588190 0.0000000 0.9659258 0.2588190 -0.0000000 0.9914449 0.1305262 -0.0000000 0.9914449 0.1305262 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 0.0000000 -0.0000000 -1.0000000 0.0000000 -0.0000000 -1.0000000 0.0000000 -0.0000000 -1.0000000 0.0000000 -0.0000000 -1.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.9914449 -0.1305262 -0.0000000 0.9659258 -0.2588190 -0.0000000 0.9659258 -0.2588190 -0.0000000 0.9914449 -0.1305262 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 0.0000000 -0.9914449 -0.1305262 0.0000000 -0.9659258 -0.2588190 0.0000000 -0.9659258 -0.2588190 0.0000000 -0.9914449 -0.1305262 -0.0000000 0.9914449 -0.1305262 0.0000000 0.9659258 -0.2588190 0.0000000 0.9659258 -0.2588190 -0.0000000 0.9914449 -0.1305262 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 0.0000000 -0.9914449 -0.1305262 0.0000000 -0.9659258 -0.2588190 0.0000000 -0.9659258 -0.2588190 0.0000000 -0.9914449 -0.1305262 -0.0000000 0.9914449 -0.1305262 0.0000000 0.9659258 -0.2588190 0.0000000 0.9659258 -0.2588190 -0.0000000 0.9914449 -0.1305262 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 0.0000000 -0.9914449 -0.1305262 0.0000000 -0.9659258 -0.2588190 0.0000000 -0.9659258 -0.2588190 0.0000000 -0.9914449 -0.1305262 -0.0000000 -0.9914449 -0.1305262 -0.0000000 -0.9659258 -0.2588190 -0.0000000 -0.9659258 -0.2588190 -0.0000000 -0.9914449 -0.1305262 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 -0.0000000 0.9914449 -0.1305262 0.0000000 0.9659258 -0.2588190 0.0000000 0.9659258 -0.2588190 -0.0000000 0.9914449 -0.1305262 0.0000000 0.8660254 -0.5000000 0.0000000 0.8660254 -0.5000000 0.0000000 0.7071068 -0.7071068 0.0000000 0.7071068 -0.7071068 0.0000000 0.5000000 -0.8660254 0.0000000 0.5000000 -0.8660254 0.0000000 0.2588190 -0.9659258 0.0000000 0.2588190 -0.9659258 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 -0.2588190 -0.9659258 0.0000000 -0.2588190 -0.9659258 0.0000000 -0.5000000 -0.8660254 0.0000000 -0.5000000 -0.8660254 0.0000000 -0.7071068 -0.7071068 0.0000000 -0.7071068 -0.7071068 0.0000000 -0.8660254 -0.5000000 0.0000000 -0.8660254 -0.5000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 1.0000000 -0.0000000 -0.0000000 0.0000000 -0.8660254 -0.5000000 0.0000000 -0.8660254 -0.5000000 0.0000000 -0.7071068 -0.7071068 0.0000000 -0.7071068 -0.7071068 0.0000000 -0.5000000 -0.8660254 0.0000000 -0.5000000 -0.8660254 0.0000000 -0.2588190 -0.9659258 0.0000000 -0.2588190 -0.9659258 0.0000000 -0.0000000 -1.0000000 0.0000000 -0.0000000 -1.0000000 0.0000000 0.2588190 -0.9659258 0.0000000 0.2588190 -0.9659258 0.0000000 0.5000000 -0.8660254 0.0000000 0.5000000 -0.8660254 0.0000000 0.7071068 -0.7071068 0.0000000 0.7071068 -0.7071068 0.0000000 0.8660254 -0.5000000 0.0000000 0.8660254 -0.5000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 0.0000000 -0.8660254 -0.5000000 0.0000000 -0.8660254 -0.5000000 0.0000000 -0.7071068 -0.7071068 0.0000000 -0.7071068 -0.7071068 0.0000000 -0.5000000 -0.8660254 0.0000000 -0.5000000 -0.8660254 0.0000000 -0.2588190 -0.9659258 0.0000000 -0.2588190 -0.9659258 0.0000000 -0.0000000 -1.0000000 0.0000000 -0.0000000 -1.0000000 0.0000000 0.2588190 -0.9659258 0.0000000 0.2588190 -0.9659258 0.0000000 0.5000000 -0.8660254 0.0000000 0.5000000 -0.8660254 0.0000000 0.7071068 -0.7071068 0.0000000 0.7071068 -0.7071068 0.0000000 0.8660254 -0.5000000 0.0000000 0.8660254 -0.5000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 0.0000000 0.8660254 -0.5000000 0.0000000 0.8660254 -0.5000000 0.0000000 -0.8660254 -0.5000000 0.0000000 -0.8660254 -0.5000000 0.0000000 -0.7071068 -0.7071068 0.0000000 -0.7071068 -0.7071068 0.0000000 -0.5000000 -0.8660254 0.0000000 -0.5000000 -0.8660254 0.0000000 -0.2588190 -0.9659258 0.0000000 -0.2588190 -0.9659258 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.2588190 -0.9659258 0.0000000 0.2588190 -0.9659258 0.0000000 0.5000000 -0.8660254 0.0000000 0.5000000 -0.8660254 0.0000000 0.7071068 -0.7071068 0.0000000 0.7071068 -0.7071068 + + + + + + + + + + + + + + +

0 1 2 1 0 3 1 3 4 4 3 5 2 6 7 6 2 1 7 6 5 7 5 3 8 9 10 9 8 11 11 8 12 12 8 13 13 8 14 10 9 15 10 15 16 10 16 17 13 14 18 18 14 19 19 14 20 20 14 21 21 14 22 22 14 23 23 14 24 23 24 25 23 25 26 23 26 27 23 27 28 23 28 29 24 14 30 30 14 31 31 14 32 32 14 33 33 14 34 34 14 35 35 14 36 36 14 37 38 39 40 39 38 41 42 43 44 43 42 45 43 45 46 43 46 47 43 47 48 43 48 49 43 49 50 43 50 51 51 50 52 51 52 53 51 53 54 51 54 55 51 55 56 51 56 57 51 57 58 58 57 59 59 57 60 60 57 61 61 57 62 62 57 63 51 58 64 51 64 65 51 65 66 51 66 67 51 67 68 51 68 69 51 69 70 51 70 71 72 73 74 73 72 75 76 77 78 77 76 79 80 81 82 81 80 83 83 80 84 84 80 85 85 80 86 86 80 87 87 80 78 87 78 77 87 77 88 87 88 89 90 91 92 91 90 93 94 93 90 93 94 95 96 95 94 95 96 97 98 96 99 96 98 97 100 99 101 99 100 98 102 101 103 101 102 100 104 103 105 103 104 102 106 105 107 105 106 104 108 107 109 107 108 106 108 110 111 110 108 109 111 112 113 112 111 110 113 114 115 114 113 112 116 117 118 117 116 119 120 119 116 119 120 121 122 121 120 121 122 123 124 122 125 122 124 123 126 125 127 125 126 124 128 127 129 127 128 126 130 129 131 129 130 128 132 131 133 131 132 130 134 133 135 133 134 132 134 136 137 136 134 135 137 138 139 138 137 136 139 140 141 140 139 138 142 143 144 143 142 145 146 145 142 145 146 147 148 147 146 147 148 149 148 150 149 150 148 151 151 152 150 152 151 153 153 154 152 154 153 155 155 156 154 156 155 157 157 158 156 158 157 159 159 160 158 160 159 161 160 162 163 162 160 161 163 164 165 164 163 162 165 166 167 166 165 164 168 169 170 169 168 171 172 171 168 171 172 173 174 173 172 173 174 175 174 176 175 176 174 177 177 178 176 178 177 179 179 180 178 180 179 181 181 182 180 182 181 183 183 184 182 184 183 185 185 186 184 186 185 187 186 188 189 188 186 187 189 190 191 190 189 188 191 192 193 192 191 190 194 195 196 195 194 197 197 194 198 198 194 199 199 194 200 200 194 201 201 194 202 202 194 203 203 194 204 204 194 205 205 194 206 207 208 209 208 207 210 211 212 213 212 211 214 212 214 215 212 215 216 212 216 217 212 217 218 212 218 219 212 219 220 212 220 221 212 221 222 212 222 223 224 225 226 225 224 227 228 229 230 229 228 231 229 231 232 229 232 233 229 233 234 229 234 235 229 235 236 229 236 237 229 237 238 229 238 239 229 239 240 241 242 243 242 241 244 245 246 247 246 245 248 249 250 251 250 249 252 252 249 253 253 249 254 254 249 255 255 249 256 256 249 257 257 249 258 258 249 259 259 249 260 260 249 261 262 263 264 263 262 265 263 265 266 263 266 267 263 267 268 263 268 269 263 269 270 263 270 271 263 271 272 263 272 273 263 273 274 275 276 277 276 275 278 279 280 281 280 279 282 283 284 285 284 283 286 284 286 287 284 287 288 284 288 289 284 289 290 284 290 291 284 291 292 284 292 293 284 293 294 284 294 295 296 297 298 297 296 299 300 301 302 301 300 303 304 305 306 305 304 307 305 307 308 305 308 309 305 309 310 305 310 311 305 311 312 305 312 313 305 313 314 305 314 315 305 315 316 317 318 319 318 317 320 321 322 323 322 321 324 325 326 327 326 325 328 328 325 329 329 325 330 330 325 331 331 325 332 332 325 333 333 325 334 334 325 335 335 325 336 336 325 337 338 339 340 339 338 341 340 342 343 342 340 339 343 344 345 344 343 342 344 346 345 346 344 347 347 348 346 348 347 349 349 350 348 350 349 351 351 352 350 352 351 353 353 354 352 354 353 355 355 356 354 356 355 357 358 356 357 356 358 359 323 359 358 359 323 322 360 325 327 325 360 361 325 361 362 325 362 363 325 363 364 325 364 365 325 365 366 325 366 367 325 367 368 325 368 369 325 369 370 319 371 372 371 319 318 372 373 374 373 372 371 375 373 376 373 375 374 377 376 378 376 377 375 379 378 380 378 379 377 381 380 382 380 381 379 383 382 384 382 383 381 385 384 386 384 385 383 387 385 386 385 387 388 302 388 387 388 302 301 305 389 306 389 305 390 390 305 391 391 305 392 392 305 393 393 305 394 394 305 395 395 305 396 396 305 397 397 305 398 398 305 399 298 400 401 400 298 297 401 402 403 402 401 400 404 402 405 402 404 403 406 405 407 405 406 404 408 407 409 407 408 406 410 409 411 409 410 408 412 411 413 411 412 410 414 413 415 413 414 412 416 414 415 414 416 417 281 417 416 417 281 280 284 418 285 418 284 419 419 284 420 420 284 421 421 284 422 422 284 423 423 284 424 424 284 425 425 284 426 426 284 427 427 284 428 429 430 431 430 429 432 432 429 433 433 429 434 434 429 435 435 429 436 436 429 437 437 429 438 438 429 439 439 429 440 440 429 441 247 442 443 442 247 246 277 444 445 444 277 276 445 446 447 446 445 444 448 446 449 446 448 447 450 449 451 449 450 448 452 451 453 451 452 450 454 453 455 453 454 452 456 455 457 455 456 454 458 457 459 457 458 456 443 458 459 458 443 442

+ + + + + + + 19.5952586 -27.6589793 13.2244094 9.2094319 -15.2455935 13.2244094 9.2094319 -27.6589793 13.2244094 19.5952586 -15.2455935 13.2244094 9.2094319 -15.2455935 13.2244094 9.2094319 -27.6589793 7.1023622 9.2094319 -27.6589793 13.2244094 9.2094319 -15.2455935 7.1023622 19.5952586 -27.6589793 13.2244094 9.2094319 -27.6589793 7.1023622 19.5952586 -27.6589793 7.1023622 9.2094319 -27.6589793 13.2244094 19.5952586 -15.2455935 7.1023622 19.5952586 -27.6589793 13.2244094 19.5952586 -27.6589793 7.1023622 19.5952586 -15.2455935 13.2244094 9.2094319 -15.2455935 13.2244094 19.5952586 -15.2455935 7.1023622 9.2094319 -15.2455935 7.1023622 19.5952586 -15.2455935 13.2244094 + + + + + + + + + + 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 -1.0000000 -0.0000000 -0.0000000 -1.0000000 -0.0000000 -0.0000000 -1.0000000 -0.0000000 -0.0000000 -1.0000000 -0.0000000 -0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 0.0000000 -1.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 1.0000000 -0.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 1.0000000 0.0000000 + + + + + + + + + + + + + + +

0 1 2 1 0 3 4 5 6 5 4 7 8 9 10 9 8 11 12 13 14 13 12 15 16 17 18 17 16 19

+ + + + + + + + + + + + + + + + 1.0000000 1.0000000 1.0000000 1.0000000 + + + + + + + + + + diff --git a/plc_bringup/meshes/hokuyo.dae b/plc_bringup/meshes/hokuyo.dae new file mode 100644 index 0000000..bc65151 --- /dev/null +++ b/plc_bringup/meshes/hokuyo.dae @@ -0,0 +1,253 @@ + + + + + Blender User + Blender 2.64.0 r51232 + + 2013-03-22T08:19:53 + 2013-03-22T08:19:53 + + Z_UP + + + + + + + 49.13434 + 1.777778 + 0.1 + 100 + + + + + + + + + + 1 1 1 + 1 + 0 + 0.00111109 + + + + + 0.000999987 + 0 + 1 + 1 + 1 + 1 + 1 + 2 + 0 + 1 + 1 + 1 + 1 + 1 + 0 + 2880 + 2 + 30.002 + 1.000799 + 0.04999995 + 29.99998 + 1 + 2 + 0 + 0 + 1 + 1 + 1 + 1 + 8192 + 1 + 1 + 0 + 1 + 1 + 1 + 3 + 0 + 0 + 0 + 0 + 45 + 0 + 1 + 1 + 1 + 3 + 0.15 + 75 + 1 + 1 + 0 + 1 + 1 + 0 + + + + + + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.020312 0.020312 0.020312 1 + + + 0.25 0.25 0.25 1 + + + 12 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.695112 0.695112 0.695112 1 + + + 0.0625 0.0625 0.0625 1 + + + 0 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + + + + + + + + + -0.02213996 0.005949974 -0.004869997 -0.02213996 0.005949974 0.004869997 -0.02164 0.009949982 -0.004999995 -0.02164 0.009949982 -0.005999982 -0.02164 0.009949982 0.004999995 -0.02164 0.009949982 0.005999982 -0.02149999 0.009949982 -0.006499946 -0.02149999 0.009949982 0.006499946 -0.02113997 0.009949982 -0.006869971 -0.02113997 0.009949982 0.006869971 -0.02063995 0.005949974 0.006999969 -0.02063995 0.009949982 -0.006999969 -0.02063995 0.009949982 0.006999969 -0.01979994 0.009949982 -0.006999969 -0.01979994 0.005949974 -0.006999969 -0.01979994 0.005949974 0.006999969 -0.01979994 0.009949982 0.006999969 -0.01838999 0.005949974 0.01014 -0.02063995 0.005949974 -0.006999969 -0.02113997 0.005949974 0.006869971 -0.02113997 0.005949974 -0.006869971 -0.02149999 0.005949974 0.006499946 -0.02149999 0.005949974 -0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02164 0.005949974 -0.004999995 -0.02164 0.005949974 -0.005999982 -0.01920998 0.01990997 0.004999995 -0.01920998 0.01990997 -0.004999995 -0.005279958 0.02695 -0.01798999 -0.009719967 0.02695 -0.01595997 -0.01014 0.02695 -0.01576995 -0.01365 0.02695 -0.01273 -0.01416999 0.02695 -0.01227998 -0.01655995 0.02695 -0.008569955 -0.01705998 0.02695 -0.007789969 -0.01822996 0.02695 -0.003779947 -0.01855999 0.02695 -0.00266999 -0.01855999 0.02695 0.00127995 -0.01855999 0.02695 0.00266999 -0.01750999 0.02695 0.006229996 -0.01705998 0.02695 0.007789969 -0.01516997 0.02695 0.01071995 -0.01416999 0.02695 0.01227998 -0.01170998 0.02695 0.01440995 -0.01014 0.02695 0.01576995 -0.007389962 0.02695 0.01703 -0.005279958 0.02695 0.01798999 -0.002529978 0.02695 0.01838999 0 0.02695 0.01874995 0.002529978 0.02695 0.01838999 0.005279958 0.02695 0.01798999 0.007389962 0.02695 0.01703 0.01014 0.02695 0.01576995 0.01170998 0.02695 0.01440995 0.01416999 0.02695 0.01227998 0.01516997 0.02695 0.01071995 0.01705998 0.02695 0.007789969 0.01750999 0.02695 0.006229996 0.01855999 0.02695 0.00266999 0.01855999 0.02695 0.00127995 0.01855999 0.02695 -0.00266999 0.01822996 0.02695 -0.003779947 0.01705998 0.02695 -0.007789969 0.01655995 0.02695 -0.008569955 0.01416999 0.02695 -0.01227998 0.01365 0.02695 -0.01273 0.01014 0.02695 -0.01576995 0.009719967 0.02695 -0.01595997 0.005279958 0.02695 -0.01798999 0.005049943 0.02695 -0.01801997 -0.005049943 0.02695 -0.01801997 0 0.02695 -0.01874995 -0.01885998 0.02594995 0.001289963 -0.01850998 0.02594995 -0.003849983 -0.02039998 0.009949982 -0.004999995 -0.02039998 0.009949982 0.004999995 -0.01848 0.02594995 0.003999948 -0.01836997 0.02572995 0.004629969 -0.01840996 0.02588999 0.004329979 -0.01838999 0.02579998 0.004489958 -0.01835 0.02544999 0.004859983 -0.01838999 0.02494996 0.004999995 -0.01835995 0.02517998 0.004969954 -0.02164 0.02494996 0.004999995 -0.02164 0.02544999 0.004869997 -0.02188998 0.02537995 0.004869997 -0.02206999 0.02519994 0.004869997 -0.02213996 0.02494996 0.004869997 -0.02164 0.02581995 0.004499971 -0.02206999 0.02569997 0.004499971 -0.02238994 0.02537995 0.004499971 -0.02249997 0.02494996 0.004499971 -0.02164 0.02594995 0.003999948 -0.02213996 0.02581995 0.003999948 -0.02263998 0.02494996 0.003999948 -0.02249997 0.02544999 0.003999948 -0.02213996 0.02581995 -0.003999948 -0.02249997 0.02544999 -0.003999948 -0.02263998 0.02494996 -0.003999948 -0.02206999 0.02569997 -0.004499971 -0.02188998 0.02537995 -0.004869997 -0.02238994 0.02537995 -0.004499971 -0.02249997 0.02494996 -0.004499971 -0.02206999 0.02519994 -0.004869997 -0.02213996 0.02494996 -0.004869997 -0.01838999 0.02494996 -0.004999995 -0.02164 0.02494996 -0.004999995 -0.01835 0.02555996 -0.004789948 -0.01835995 0.02526998 -0.004949986 -0.01835 0.02543997 -0.00484997 -0.01836997 0.02577996 -0.004559993 -0.02164 0.02544999 -0.004869997 -0.02164 0.02581995 -0.004499971 -0.01848 0.02594995 -0.003999948 -0.02164 0.02594995 -0.003999948 -0.01841998 0.02590996 -0.004269957 -0.02001994 0.01871997 -0.004999995 -0.01986998 0.01879996 -0.004999995 -0.02025997 0.01844996 -0.004999995 -0.02039998 0.01794999 -0.004999995 -0.02037996 0.01813995 -0.004999995 -0.02039998 0.01794999 0.004999995 -0.02033996 0.01826995 0.004999995 -0.01993995 0.01877999 0.004999995 -0.01936 0.01894998 0.004999995 -0.01962 0.01892 0.004999995 -0.01879996 0.01894998 0.006679952 -0.01845997 0.01894998 0.007699966 -0.01936 0.01894998 -0.004999995 -0.01971 0.01888996 -0.004999995 -0.01845997 0.01894998 -0.007699966 -0.01668 0.01894998 -0.01102995 -0.01141995 0.01894998 -0.01641994 -7.8e-4 0.01894998 -0.01997995 0 0.01894998 -0.01993995 0.002989947 0.01894998 -0.01976996 0.01315999 0.01894998 -0.01505994 0.01454997 0.01894998 -0.01358997 0.01576 0.01894998 -0.01230996 0.01990997 0.01894998 0.001369953 0.01990997 0.01894998 -0.001889944 0.01919996 0.01894998 0.005609989 0.01779997 0.01894998 0.009119987 0.00792998 0.01894998 0.01826995 0.01008999 0.01894998 0.01726996 -0.007959961 0.01894998 0.01832997 -0.004529953 0.01894998 0.01947999 -0.01668 0.01894998 0.01102995 -0.01892 0.01881998 0.007889986 -0.02024996 0.01843994 0.004999995 -0.02017998 0.01855999 0.004999995 -0.00812 0.01894998 -0.01827996 0.006659984 0.01894998 -0.01885998 0.01576 0.01894998 0.01230996 0.002989947 0.01894998 0.01976996 0.002719998 0.01894998 0.01978999 -0.01141995 0.01894998 0.01641994 -0.01256996 0.01894998 0.01545 -0.01767998 0.01894998 -0.009159982 -0.01892 0.01881998 -0.007889986 -0.01937997 0.01794999 -0.008079946 -0.01830995 0.01794999 -0.01009994 -0.01925998 0.01844996 -0.008029997 -0.01458996 0.01894998 -0.01362997 -0.01709997 0.01881998 -0.01130998 -0.01750999 0.01794999 -0.01159 -0.0151 0.01794999 -0.01457995 -0.01739996 0.01844996 -0.01150995 -0.01431 0.01894998 -0.01397997 -0.01466 0.01881998 -0.01432996 -0.01501995 0.01794999 -0.01467996 -0.01491999 0.01844996 -0.01457995 -0.01034998 0.01894998 -0.01701998 -0.008319973 0.01881998 -0.01872998 -0.01169997 0.01881998 -0.01682996 -0.01198995 0.01794999 -0.01723998 -0.01190996 0.01844996 -0.01712995 -8.2e-4 0.01794999 -0.02098 -0.004759967 0.01794999 -0.02044999 -0.004729986 0.01844996 -0.02031999 -0.005689978 0.01794999 -0.02013999 -0.01073998 0.01794999 -0.01794999 -0.008529961 0.01794999 -0.01918995 -0.008469998 0.01844996 -0.01906996 -2.5e-4 0.01794999 -0.02094995 -8.2e-4 0.01844996 -0.02085 -8.1e-4 0.01881998 -0.02047997 -0.005379974 0.01894998 -0.01919996 -0.004649996 0.01881998 -0.01996999 -0.004529953 0.01894998 -0.01947999 0.005379974 0.01894998 -0.01917999 0.006829977 0.01881998 -0.01932996 0.003069996 0.01881998 -0.02026998 0.003139972 0.01794999 -0.02075999 0.003119945 0.01844996 -0.02063 0.01381999 0.01794999 -0.01580995 0.01059997 0.01794999 -0.01813 0.01052999 0.01844996 -0.01800996 0.01034998 0.01794999 -0.01824998 0.005209982 0.01794999 -0.02024996 0.006989955 0.01794999 -0.01979994 0.006949961 0.01844996 -0.01966995 0.01469999 0.01794999 -0.01488 0.01372998 0.01844996 -0.01570999 0.01348996 0.01881998 -0.01542997 0.01008999 0.01894998 -0.01726996 0.01034998 0.01881998 -0.01769995 0.01036995 0.01894998 -0.01705998 0.01774996 0.01894998 -0.009199976 0.01823997 0.01881998 -0.009349942 0.01615995 0.01881998 -0.01261997 0.01655 0.01794999 -0.01291996 0.01644998 0.01844996 -0.01283997 0.02090996 0.01794999 -0.00198996 0.02023994 0.01794999 -0.005469977 0.02002996 0.01844996 -0.005849957 0.02015995 0.01794999 -0.005889952 0.01807999 0.01794999 -0.01052999 0.01868999 0.01794999 -0.009579956 0.01857 0.01844996 -0.009519994 0.02090996 0.01794999 0 0.02076995 0.01844996 -0.001969993 0.02041 0.01881998 -0.001939952 0.01919996 0.01894998 -0.005609989 0.01967996 0.01881998 -0.00575 0.01949995 0.01894998 -0.004049956 0.01990997 0.01894998 0.001889944 0.02041 0.01881998 0.001939952 0.02090996 0.01794999 0.00198996 0.02023994 0.01794999 0.005469977 0.02076995 0.01844996 0.001969993 0.01876997 0.01894998 0.006669998 0.01967996 0.01881998 0.00575 0.02015995 0.01794999 0.005889952 0.02002996 0.01844996 0.005849957 0.01627999 0.01894998 0.0115 0.01615995 0.01881998 0.01261997 0.01823997 0.01881998 0.009349942 0.01868999 0.01794999 0.009579956 0.01857 0.01844996 0.009519994 0.01059997 0.01794999 0.01813 0.01381999 0.01794999 0.01580995 0.01372998 0.01844996 0.01570999 0.01469999 0.01794999 0.01488 0.01807999 0.01794999 0.01052999 0.01655 0.01794999 0.01291996 0.01644998 0.01844996 0.01283997 0.01034998 0.01794999 0.01824998 0.01052999 0.01844996 0.01800996 0.01034998 0.01881998 0.01769995 0.01315999 0.01894998 0.01505994 0.01348996 0.01881998 0.01542997 0.01258999 0.01894998 0.01546996 0.006659984 0.01894998 0.01885998 0.003069996 0.01881998 0.02026998 0.006829977 0.01881998 0.01932996 0.006989955 0.01794999 0.01979994 0.006949961 0.01844996 0.01966995 -0.004759967 0.01794999 0.02044999 -8.2e-4 0.01794999 0.02098 -8.2e-4 0.01844996 0.02085 -2.5e-4 0.01794999 0.02094995 0.005209982 0.01794999 0.02024996 0.003139972 0.01794999 0.02075999 0.003119945 0.01844996 0.02063 -0.005689978 0.01794999 0.02013999 -0.004729986 0.01844996 0.02031999 -0.004649996 0.01881998 0.01996999 -7.8e-4 0.01894998 0.01997995 -8.1e-4 0.01881998 0.02047997 -0.002709984 0.01894998 0.01972997 -0.00812 0.01894998 0.01827996 -0.01169997 0.01881998 0.01682996 -0.008319973 0.01881998 0.01872998 -0.008529961 0.01794999 0.01918995 -0.008469998 0.01844996 0.01906996 -0.01750999 0.01794999 0.01159 -0.0151 0.01794999 0.01457995 -0.01491999 0.01844996 0.01457995 -0.01501995 0.01794999 0.01467996 -0.01073998 0.01794999 0.01794999 -0.01198995 0.01794999 0.01723998 -0.01190996 0.01844996 0.01712995 -0.01937997 0.01794999 0.008079946 -0.01830995 0.01794999 0.01009994 -0.01925998 0.01844996 0.008029997 -0.01739996 0.01844996 0.01150995 -0.01709997 0.01881998 0.01130998 -0.01431 0.01894998 0.01397997 -0.01466 0.01881998 0.01432996 -0.01629996 0.01894998 0.0115 -0.01014 0.03504997 -0.01576995 -0.005279958 0.03504997 0.01798999 0 0.03504997 0.01874995 0.005279958 0.03504997 0.01798999 0.01014 0.03504997 0.01576995 0.01416999 0.03504997 0.01227998 0.01705998 0.03504997 0.007789969 0.01855999 0.03504997 0.00266999 0.01855999 0.03504997 -0.00266999 -0.01014 0.03504997 0.01576995 -0.01416999 0.03504997 0.01227998 -0.01705998 0.03504997 0.007789969 -0.01855999 0.03504997 0.00266999 -0.01855999 0.03504997 -0.00266999 -0.01705998 0.03504997 -0.007789969 -0.01416999 0.03504997 -0.01227998 -0.005279958 0.03504997 -0.01798999 0 0.03504997 -0.01874995 0.005279958 0.03504997 -0.01798999 0.01014 0.03504997 -0.01576995 0.01416999 0.03504997 -0.01227998 0.01705998 0.03504997 -0.007789969 0.001609981 0.03494995 -0.01717996 -0.003309965 0.03504997 -0.01692998 -0.003309965 0.03494995 -0.01692998 0.001759946 0.03494995 -0.01713997 0.001969993 0.03494995 -0.01708996 0.001609981 0.03504997 -0.01717996 0.004629969 0.03494995 -0.01644998 0.006399989 0.03494995 -0.01601999 0.007199943 0.03494995 -0.01555997 0.007989943 0.03494995 -0.0151 0.006399989 0.03504997 -0.01601999 0.009659945 0.03494995 -0.01413995 0.01066994 0.03494995 -0.01354998 0.01066994 0.03504997 -0.01354998 0.01512998 0.03494995 -0.007909953 0.01104998 0.03494995 -0.01314997 0.01406997 0.03504997 -0.009979963 0.01406997 0.03494995 -0.009979963 0.01695996 0.03494995 -0.002209961 0.01631999 0.03494995 -0.005589962 0.01631999 0.03504997 -0.005589962 0.01652997 0.03494995 -0.004479944 -0.007969975 0.03494995 -0.01529997 -0.007969975 0.03504997 -0.01529997 -0.009269952 0.03494995 -0.01454997 -0.01002997 0.03494995 0.01388996 -0.01088994 0.03494995 0.01314997 -0.01299995 0.03494995 0.01133996 -0.01299995 0.03504997 0.01133996 -0.01378995 0.03494995 0.01010996 -0.01567995 0.03494995 0.007189989 -0.01567995 0.03504997 0.007189989 -0.01626998 0.03494995 0.005199968 -0.01706999 0.03494995 0.002459943 -0.01706999 0.03494995 0.002019941 -0.01706999 0.03494995 9.7e-4 -0.01706999 0.03494995 6.9e-4 -0.01706999 0.03504997 0.002459943 -0.01706999 0.03494995 -0.002209961 -0.01706999 0.03494995 -0.002459943 -0.01706999 0.03504997 -0.002459943 -0.01647996 0.03494995 -0.004479944 -0.01567995 0.03494995 -0.007189989 -0.01567995 0.03504997 -0.007189989 -0.01521998 0.03494995 -0.007909953 -0.01299995 0.03504997 -0.01133996 -0.01299995 0.03494995 -0.01133996 -0.01002997 0.03494995 -0.01388996 -0.01088994 0.03494995 -0.01314997 -0.009269952 0.03504997 -0.01454997 -0.009269952 0.03494995 0.01454997 -0.009269952 0.03504997 0.01454997 0.01718997 0.03494995 9.7e-4 0.01699 0.03494995 0.002019941 0.01638996 0.03494995 0.005199968 0.01631999 0.03494995 0.005589962 0.01631999 0.03504997 0.005589962 0.01104998 0.03494995 0.01314997 0.01406997 0.03494995 0.009979963 0.01406997 0.03504997 0.009979963 0.01394999 0.03494995 0.01010996 0.01066994 0.03494995 0.01354998 0.009659945 0.03494995 0.01413995 0.01066994 0.03504997 0.01354998 0.007199943 0.03494995 0.01555997 0.006399989 0.03494995 0.01601999 0.006399989 0.03504997 0.01601999 0.004629969 0.03494995 0.01644998 0.001969993 0.03494995 0.01708996 0.001759946 0.03494995 0.01713997 0.001609981 0.03494995 0.01717996 0.001609981 0.03504997 0.01717996 -0.003309965 0.03494995 0.01692998 -0.007969975 0.03504997 0.01529997 -0.007969975 0.03494995 0.01529997 -0.003309965 0.03504997 0.01692998 0.01722997 0.03504997 7.5e-4 0.01722997 0.03494995 7.5e-4 0.01722997 0.03494995 -7.5e-4 0.01722997 0.03504997 -7.5e-4 -0.02063995 0.005949974 0.006999969 -0.02213996 0.005949974 0.004869997 -0.02213996 0.005949974 -0.004869997 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.02499997 -0.03504997 -0.023 -0.02499997 -0.03504997 0.023 -0.02472996 -0.03504997 -0.02399998 -0.02472996 -0.03504997 0.02399998 -0.02399998 -0.03504997 -0.02472996 -0.02399998 -0.03504997 0.02472996 -0.02363997 0.003949999 -0.02164 -0.02363997 0.003949999 0.02164 -0.02363997 0.00544995 -0.02164 -0.02363997 0.00544995 0.02164 -0.02357 0.005699992 -0.02164 -0.02357 0.005699992 0.02164 -0.02338999 0.005879998 -0.02164 -0.02338999 0.005879998 0.02164 -0.02336996 0.003959953 0.02263998 -0.02336996 0.00544995 -0.02263998 -0.02336996 0.00544995 0.02263998 -0.02336996 0.003959953 -0.02263998 -0.02331 0.005699992 -0.02260994 -0.02331 0.005699992 0.02260994 -0.02314996 0.005879998 -0.02250999 -0.02314996 0.005879998 0.02250999 -0.02313995 0.005949974 -0.02164 -0.02313995 0.005949974 0 -0.02313995 0.005949974 0.02164 -0.023 -0.03504997 -0.02499997 -0.023 -0.03504997 0.02499997 -0.02293998 0.005949974 -0.02238994 -0.02293998 0.005949974 0.02238994 0 -0.03504997 0 -0.02263998 0.003959953 0.02336996 -0.02263998 0.003959953 -0.02336996 -0.02263998 0.00544995 -0.02336996 -0.02263998 0.00544995 0.02336996 -0.02260994 0.005699992 -0.02331 -0.02260994 0.005699992 0.02331 -0.02250999 0.005879998 -0.02314996 -0.02250999 0.005879998 0.02314996 0 0.005949974 -0.02313995 -0.02164 0.005949974 -0.02313995 -0.02238994 0.005949974 -0.02293998 -0.02238994 0.005949974 0.02293998 -0.02164 0.003949999 -0.02363997 -0.02164 0.003949999 0.02363997 -0.02164 0.00544995 -0.02363997 -0.02164 0.00544995 0.02363997 -0.02164 0.005699992 -0.02357 -0.02164 0.005699992 0.02357 -0.02164 0.005879998 -0.02338999 -0.02164 0.005879998 0.02338999 -0.02164 0.005949974 0.02313995 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.02213996 0.005949974 -0.004869997 0 0.005949974 0.02313995 0.02164 0.003949999 -0.02363997 0.02164 0.003949999 0.02363997 0.02164 0.00544995 -0.02363997 0.02164 0.00544995 0.02363997 0.02164 0.005699992 -0.02357 0.02164 0.005699992 0.02357 0.02164 0.005879998 -0.02338999 0.02164 0.005879998 0.02338999 0.02164 0.005949974 -0.02313995 0.02164 0.005949974 0.02313995 0.02238994 0.005949974 -0.02293998 0.02238994 0.005949974 0.02293998 0.02250999 0.005879998 -0.02314996 0.02250999 0.005879998 0.02314996 0.02260994 0.005699992 -0.02331 0.02260994 0.005699992 0.02331 0.02263998 0.003959953 -0.02336996 0.02263998 0.003959953 0.02336996 0.02263998 0.00544995 -0.02336996 0.02263998 0.00544995 0.02336996 0.02293998 0.005949974 -0.02238994 0.02293998 0.005949974 0.02238994 0.023 -0.03504997 -0.02499997 0.023 -0.03504997 0.02499997 0.02313995 0.005949974 -0.02164 0.02313995 0.005949974 0 0.02313995 0.005949974 0.02164 0.02314996 0.005879998 -0.02250999 0.02314996 0.005879998 0.02250999 0.02331 0.005699992 -0.02260994 0.02331 0.005699992 0.02260994 0.02336996 0.003959953 -0.02263998 0.02336996 0.003959953 0.02263998 0.02336996 0.00544995 -0.02263998 0.02336996 0.00544995 0.02263998 0.02338999 0.005879998 -0.02164 0.02338999 0.005879998 0.02164 0.02357 0.005699992 -0.02164 0.02357 0.005699992 0.02164 0.02363997 0.003949999 -0.02164 0.02363997 0.003949999 0.02164 0.02363997 0.00544995 -0.02164 0.02363997 0.00544995 0.02164 0.02399998 -0.03504997 -0.02472996 0.02399998 -0.03504997 0.02472996 0.02472996 -0.03504997 -0.02399998 0.02472996 -0.03504997 0.02399998 0.02499997 -0.03504997 -0.023 0.02499997 -0.03504997 0.023 0 0.03504997 0 -0.02213996 0.005949974 0.004869997 -0.02063995 0.005949974 0.006999969 -0.01979994 0.005949974 0.006999969 -0.01838999 0.005949974 0.01014 -0.02113997 0.005949974 0.006869971 -0.02149999 0.005949974 0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.01979994 0.005949974 0.006999969 -0.01838999 0.005949974 0.01014 -0.02113997 0.005949974 0.006869971 -0.02149999 0.005949974 0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.02063995 0.005949974 0.006999969 -0.02213996 0.005949974 0.004869997 -0.02213996 0.005949974 -0.004869997 -0.01979994 0.005949974 0.006999969 -0.01838999 0.005949974 0.01014 -0.02113997 0.005949974 0.006869971 -0.02149999 0.005949974 0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.00945729 0.003627836 -0.001710772 0 -0.01554995 0.02431994 0 0.005949974 0 + + + + + + + + + + 0 -1 0 0 -1 0 -0.9492978 0 -0.3143782 -0.9492978 0 0.3143782 -0.7785766 0.001171708 0.6275485 -0.7808654 0.001171231 0.6246982 -0.6209343 -0.001221954 0.7838616 -0.6453004 -0.01542252 0.7637733 -0.3964945 -0.0151537 0.9179121 -0.4893132 0.006718158 0.8720822 -0.4938785 0.006718337 0.8695049 -0.1431924 -0.008457064 0.9896588 -0.316222 0.006060898 0.9486659 -0.3172233 0.0060606 0.9483315 0.2392156 0.007879972 0.9709345 0.119358 -0.01385879 0.9927546 0.0559594 0.00282979 0.9984291 0.1191433 0.00413686 0.9928685 0.05255794 0.004160761 0.9986092 -0.1333157 -0.004680752 0.9910625 0.2450903 0.007883667 0.9694681 0.3747907 -0.01294112 0.9270192 0.4188861 0.001862525 0.9080369 0.4327281 0.001863181 0.9015226 0.6034716 -0.002766609 0.7973797 0.5845207 -0.01302987 0.8112742 0.7900453 -0.01468151 0.6128727 0.7272006 0.007067918 0.6863886 0.7263449 0.007068157 0.6872941 0.9823358 0.002611696 0.1871086 0.9289886 -0.004128932 0.3700852 0.9230861 -0.009820222 0.3844679 0.8414472 0.006709396 0.5402976 0.8421885 0.006709873 0.5391413 0.9819628 0.002612411 0.1890563 0.9911401 -0.01446169 0.1320315 0.9999719 0.007499694 0 0.9999719 0.007499694 0 0.9911401 -0.01446169 -0.1320315 0.9819628 0.002612411 -0.1890563 0.9823358 0.002611696 -0.1871086 0.9231205 -0.004674613 -0.3844823 0.9289375 -0.01127177 -0.3700649 0.7900453 -0.01468151 -0.6128727 0.8421885 0.006709873 -0.5391413 0.8414472 0.006709396 -0.5402976 0.4327281 0.001863181 -0.9015226 0.5845689 -0.002225697 -0.8113411 0.6034404 -0.0105375 -0.7973385 0.7263449 0.007068157 -0.6872941 0.7272006 0.007067918 -0.6863886 0.4188861 0.001862525 -0.9080369 0.3747907 -0.01294112 -0.9270192 0.2450903 0.007883667 -0.9694681 0.2392156 0.007879972 -0.9709345 0.119358 -0.01385879 -0.9927546 0.0559594 0.00282979 -0.9984291 0.1191433 0.00413686 -0.9928685 0.05255794 0.004160761 -0.9986092 -0.1431956 -0.005152344 -0.989681 -0.1333114 -0.009266734 -0.9910309 -0.3964945 -0.0151537 -0.9179121 -0.3172233 0.0060606 -0.9483315 -0.316222 0.006060898 -0.9486659 -0.7808654 0.001171231 -0.6246982 -0.6453768 -8.88654e-4 -0.7638639 -0.6208926 -0.01165908 -0.783809 -0.4938785 0.006718337 -0.8695049 -0.4893132 0.006718158 -0.8720822 -0.7785766 0.001171708 -0.6275485 -0.8049004 -0.01266139 -0.5932749 -0.881016 0.007450222 -0.4730277 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.9471516 0.1680965 0.2732169 -0.407418 0.1514139 -0.9006023 -0.9422059 0.2770094 -0.1884511 -0.9113164 0.1482697 -0.3840813 -0.9315509 0.1626105 -0.3252239 -0.7991091 -0.3146805 -0.5122509 -0.9081746 0.2506373 -0.3352608 -0.9111543 0.1456485 -0.3854666 -0.928664 0.2243673 -0.2953684 -0.925579 0.2859641 -0.2480482 -0.9177774 0.2868903 0.2745518 -0.9178981 0.2856691 0.2754201 -0.9143468 0.1813663 0.3620446 -0.9384292 0.1466302 0.31281 -0.9311535 0.1683419 0.3234411 -0.901751 0.1467134 0.4065961 -0.9103941 0.1680585 0.3780726 -0.912325 0.3225979 0.2521777 -0.9188216 0.2869244 0.2710005 -0.9848498 0.1048805 0.138097 -0.9578264 0.2873474 0 -0.9559739 0.2861407 -0.06509518 -0.9987523 -0.04993659 0 -0.9210147 0.2770513 -0.2738146 -0.4861897 0.1523008 -0.8604789 -0.4091901 0.1777053 -0.8949773 -0.3149025 0.1457877 -0.9378605 -0.1278769 0.1498839 -0.9803991 -0.309273 0.1512408 -0.9388697 -0.141013 0.1688628 -0.9755002 -0.1303948 0.1630746 -0.977959 -0.4837509 0.1474936 -0.8626881 -0.636475 0.163087 -0.7538581 -0.645197 0.1696269 -0.744948 -0.7708169 0.1599442 -0.6166515 -0.6459589 0.159875 -0.746443 -0.7711567 0.1450923 -0.6198915 -0.8274903 0.1763324 -0.533073 -0.8709936 0.1562899 -0.4657721 -0.9149418 0.1555567 -0.3724025 -0.7441748 0.5367867 -0.3975726 -0.9384292 0.1466302 -0.3128098 -0.9364062 0.1603595 0.3121352 -0.8722791 0.1474033 0.4662632 -0.8271023 0.1767897 0.5335236 -0.7664864 0.1686882 0.6197121 -0.8297607 0.1690638 0.5318973 -0.7716946 0.1450895 0.6192225 -0.6450954 0.1696101 0.7450398 -0.6363618 0.1663306 0.7532449 -0.64563 0.1662972 0.7453235 -0.6361207 0.1630551 0.754164 -0.4856274 0.1477051 0.8615968 -0.4099487 0.177209 0.8947284 -0.2937462 0.1736751 0.9399735 -0.4077602 0.1746891 0.8962228 -0.3145152 0.1452308 0.9380769 -0.1418845 0.1683772 0.9754577 -0.1341822 0.1666406 0.976845 -0.1389042 0.1666364 0.9761855 -0.1267469 0.1627997 0.9784843 0.1395421 0.1371724 0.980669 0.05338537 0.1733645 0.9834097 0.07274723 0.1735997 0.9821257 0.1416881 0.1761806 0.9741073 0.2381073 0.1455035 0.9602779 0.4082419 0.1680009 0.8972816 0.4154286 0.1666471 0.8942303 0.4107152 0.1666655 0.8964015 0.4143497 0.165222 0.894995 0.6486326 0.1363442 0.7487897 0.575868 0.1693211 0.7998164 0.575499 0.1693159 0.8000832 0.6442187 0.1772168 0.7440273 0.7189208 0.1454344 0.6797072 0.8300612 0.16697 0.5320899 0.8296917 0.1670521 0.5326398 0.8285282 0.1670665 0.5344434 0.8312138 0.1651425 0.5308593 0.9521918 0.1337416 0.2746708 0.9171259 0.1644181 0.3631072 0.914055 0.1643574 0.3707966 0.9438654 0.1778166 0.2783871 0.9717582 0.1459009 0.1854698 0.9860588 0.1663975 0 0.9860587 0.1663975 0 0.9860587 0.1663975 0 0.9860587 0.1663975 0 0.9500489 0.1327823 -0.2824464 0.9697747 0.1601643 -0.1840775 0.9693279 0.1601721 -0.1864089 0.9447124 0.1775989 -0.2756393 0.830715 0.1654286 -0.5315507 0.8288937 0.166745 -0.533977 0.7189707 0.1454441 -0.6796524 0.6466634 0.1530798 -0.747257 0.717962 0.1537514 -0.6788895 0.6443232 0.1772074 -0.743939 0.5763537 0.1462174 -0.8040131 0.4070715 0.1568021 -0.8998366 0.5925795 0.1567866 -0.7901059 0.4147536 0.1656929 -0.8947209 0.4098841 0.1681987 -0.8964955 0.2399455 0.1457665 -0.9597803 0.1279237 0.1474865 -0.9807565 0.2369926 0.1489789 -0.9600207 0.1408129 0.1768286 -0.9741167 0.05614864 0.1468989 -0.9875566 0.05065917 0.1469421 -0.987847 -0.1424711 -3.79589e-4 -0.9897989 -0.129338 0 -0.9916005 -0.4158082 0 -0.9094523 -0.4154945 -2.07829e-4 -0.9095956 -0.4121701 0 -0.911107 -0.6546844 0 -0.7559022 -0.6546447 -3.01789e-5 -0.7559368 -0.6543759 0 -0.7561693 -0.840663 0 -0.5415588 -0.8408741 2.12565e-4 -0.5412307 -0.8418785 0 -0.5396671 -0.9599732 0 -0.280092 -0.9596633 -5.69179e-4 -0.2811512 -0.9585365 0 -0.2849697 -1 0 0 -1 0 0 -1 0 0 -0.9591508 0 0.2828954 -0.9596631 -8.33003e-4 0.2811512 -0.9608235 0 0.2771607 -0.8403387 0 0.5420618 -0.840874 -4.25545e-4 0.5412307 -0.8418794 0 0.5396656 -0.6545795 0 0.7559933 -0.6546447 -3.46783e-5 0.7559368 -0.6547469 0 0.7558482 -0.4165412 0 0.9091168 -0.4154945 4.29833e-4 0.9095956 -0.4141279 0 0.9102187 -0.1439396 0 0.9895865 -0.142471 5.09058e-4 0.9897989 -0.1408738 0 0.9900277 0.1408738 0 0.9900277 0.142471 5.09058e-4 0.9897989 0.1439396 0 0.9895865 0.4141279 0 0.9102187 0.4154945 4.29833e-4 0.9095956 0.4165412 0 0.9091168 0.6547469 0 0.7558482 0.6546447 -3.46783e-5 0.7559368 0.6545795 0 0.7559933 0.8418794 0 0.5396656 0.840874 -4.25545e-4 0.5412307 0.8403387 0 0.5420618 0.9608235 0 0.2771607 0.9596631 -8.33003e-4 0.2811512 0.9591508 0 0.2828954 1 0 0 1 0 0 1 0 0 0.9585365 0 -0.2849697 0.9596633 -5.69179e-4 -0.2811512 0.9599732 0 -0.280092 0.8418785 0 -0.5396671 0.8408741 2.12565e-4 -0.5412307 0.840663 0 -0.5415588 0.6543759 0 -0.7561693 0.6546447 -3.01789e-5 -0.7559368 0.6546844 0 -0.7559022 0.4121701 0 -0.911107 0.4154945 -2.07829e-4 -0.9095956 0.4158082 0 -0.9094523 0.129338 0 -0.9916005 0.1424711 -3.79589e-4 -0.9897989 0.1430675 0 -0.9897129 -0.1430675 0 -0.9897129 -1 0 0 -1 0 0 -1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0.9838701 0.1788842 -0.007730364 0.9677938 0.2516258 -0.008649408 0.7070808 0.7070799 0.006879746 0.8947982 0.4464178 0.006871819 0.8719238 0.4895934 0.002814829 0.377206 0.9261251 -0.008792817 0.2516237 0.9677851 0 0.1293374 0.9916006 -0.9629649 0 0.269627 -0.9629649 0 0.269627 -0.251012 0.07028329 0.965429 -0.1845505 0.1845505 0.9653404 -0.07028359 0.2510112 0.9654293 -0.1942211 0.6936423 0.6936414 -0.1935422 0.6935194 0.6939541 -0.5112335 0.5112336 0.690855 -0.5112314 0.5112344 0.690856 -0.6932809 0.1941184 0.6940315 -0.7049736 0.1803423 0.6859219 -0.2607492 0.9343423 0.2429284 -0.243086 0.9349478 0.2584215 -0.682882 0.6828859 0.2594976 -0.6944488 0.6756823 0.2473751 -0.9403539 0.2405559 0.2405565 -0.9297617 0.2603295 0.2603303 -0.2516335 0.9678226 0 -0.2516335 0.9678226 0 -0.7167247 0.6973562 0 -0.7167247 0.6973562 0 -0.962965 0.2696263 0 -0.962965 0.2696263 0 -0.7167251 0 -0.6973558 -0.9629649 0 -0.269627 -0.9629649 0 -0.269627 -0.07028359 0.2510112 -0.9654293 -0.1845506 0.1845506 -0.9653404 -0.2440263 0.938564 -0.2440261 -0.2595868 0.9301772 -0.2595866 -0.6923445 0.673635 -0.2586021 -0.6852509 0.685255 -0.2466917 -0.9343009 0.2616005 -0.2421712 -0.9350127 0.2391895 -0.2618005 -0.1936007 0.6937291 -0.6937282 -0.1941202 0.693282 -0.6940299 -0.5112318 0.5112348 -0.6908552 -0.5112326 0.5112326 -0.6908563 -0.6994224 0.1789222 -0.6919503 -0.7027144 0.1967598 -0.6837237 -0.251012 0.07028329 -0.965429 0 0.1543763 -0.9880121 -0.009795784 0.2516214 -0.9677761 -0.007334709 0.722306 -0.6915347 0.00679636 0.4472005 -0.8944079 0.006780803 0.5067117 -0.8620889 -0.009076535 0.9677828 -0.2516229 0 0.9892032 -0.1465499 -0.2263051 0.7072249 0.6697902 0.3015977 0.696307 0.6513029 0.7167264 0.6973544 0 0.5183387 0.69911 -0.4925142 0.03862148 0.6995103 -0.7135781 -0.1644762 0.9669772 0.1946862 -0.1248043 0.9671578 0.2214269 0.01370066 0.9675313 0.2523798 0.06004744 0.9683741 0.2421693 0.1849305 0.9670732 0.1748434 0.1059339 0.9677575 -0.2285243 -0.1261059 0.966584 -0.2231875 -0.199501 0.9668129 -0.1596003 -0.6712515 0.704275 -0.2311238 -0.4654791 0.8727707 -0.1469718 -0.4844728 0.8612704 -0.1532949 -0.8913083 0.3450198 -0.2941612 -0.915989 0.251125 -0.3128905 -0.944593 0.09943693 -0.3128201 -0.9346077 0.1752427 0.3095133 -0.9163941 0.251105 0.3117182 -0.8494461 0.4497035 0.2760577 -0.6587777 0.7186641 0.2225626 -0.11454 0.9926847 0.03817999 -0.2079164 0.9745261 0.08408081 -0.2403051 0.9673867 0.08010166 -0.2239108 0.9672326 0.119688 -0.2239329 0.9672339 -0.1196353 -0.2369856 0.9674997 -0.08821576 -0.168695 0.984063 -0.05623167 -0.220901 0.9681158 -0.1181289 -0.195109 0.9681603 -0.1568376 -0.1642051 0.9670629 -0.194489 -0.1243403 0.9671458 -0.2217399 -0.03328311 0.9677709 -0.249623 0.01294058 0.9675522 -0.2523397 0.01371669 0.9672559 -0.2534325 0.01418453 0.9682756 -0.2494817 0.05979275 0.9683793 -0.2422114 0.1485582 0.966945 -0.2072386 0.1849181 0.9670735 -0.1748546 0.1827625 0.9676989 -0.173657 0.181624 0.9682638 -0.1716915 0.2103747 0.9683087 -0.1346127 0.2516335 0.9678226 0 0.2516335 0.9678226 0 0.2516335 0.9678227 0 0.2469634 0.9678903 -0.04687726 0.2482241 0.9675434 0.04737603 0.2343087 0.9675045 0.09504997 0.2367252 0.9670455 0.09372389 0.21423 0.9671535 0.1368191 0.1064419 0.9675605 0.2291214 0.1058055 0.9677801 0.2284879 0.1058276 0.9677543 0.2285876 0.1467888 0.9679301 0.2038734 -0.0766099 0.966454 0.245148 -0.08143335 0.9670985 0.2410164 -0.07958346 0.9681548 0.2373665 -0.03413015 0.9680389 0.2484672 -0.1947117 0.9681448 0.1574264 -0.2211804 0.9681038 0.1177042 -0.6276769 0.7031698 0.3340269 -0.6718416 0.7042246 0.2295574 -0.8316648 0.4851421 0.270131 -0.224141 0.7071188 -0.6706294 -0.08158397 0.9665949 -0.242978 -0.08118349 0.9673035 -0.240277 -0.230294 0.6945442 -0.6815959 -0.09658795 0.6949279 -0.7125629 -0.1300739 0.2494957 -0.9596003 0.05087167 0.2513077 -0.9665695 0.3019366 0.6962896 -0.6511644 0.06135189 0.9674767 -0.2454073 0.1061488 0.9675673 -0.2292293 0.3024179 0.6942894 -0.653074 0.4201316 0.6941274 -0.584531 0.5638182 0.2583763 -0.7844429 0.7015674 0.2590524 -0.6638488 0.657499 0.7065963 -0.2615662 0.6635753 0.6994388 -0.2654304 0.7016292 0.6998641 -0.133816 0.5189352 0.6991608 0.4918134 0.2101477 0.9683169 0.1349092 0.1812267 0.9682484 0.1721977 0.5147917 0.7040795 0.4891439 0.4141255 0.7046437 0.5761747 0.562515 0.2665852 0.7826297 0.4170309 0.2669032 0.8688198 0.03983807 0.6995611 0.7134614 0.01843094 0.9683725 0.2488275 0.01364505 0.9676027 0.2521088 0.03827118 0.7060714 0.7071057 -0.09523808 0.7051784 0.7026044 -0.1298174 0.2567928 0.9577082 -0.305629 0.2567279 0.916887 -0.4603919 0.7019324 0.5434429 -0.1641054 0.967066 0.194558 -0.164548 0.966937 0.1948249 -0.463243 0.6961148 0.5484799 -0.5580259 0.6967077 0.4507831 -0.8506854 0.2602313 -0.4567428 -0.2229626 0.9675791 -0.1186526 -0.6337479 0.6961472 -0.3372576 -0.6271286 0.7031058 -0.3351895 -0.8501825 0.2659004 -0.454408 -0.8554773 0.2506282 -0.4531491 -0.7517282 0.2706233 -0.6013882 -0.1978173 0.9671215 -0.159826 -0.5579887 0.6967101 -0.4508255 -0.5582559 0.6964031 -0.4509689 -0.7490752 0.2696682 -0.6051161 -0.7513393 0.2621917 -0.6055946 -0.1644515 0.9669765 -0.1947106 -0.4595423 0.7019816 -0.5440982 -0.464033 0.6961706 -0.5477408 -0.6238481 0.261821 -0.7363854 -0.621176 0.2712791 -0.7352197 -0.1242595 0.9673138 -0.221051 -0.3465486 0.7069951 -0.6164916 -0.3499536 0.7017597 -0.6205367 -0.4731478 0.2687683 -0.8389843 -0.4767771 0.2609524 -0.839397 -0.1288474 0.2567702 -0.9578452 -0.305629 0.2567279 -0.9168869 -0.3062461 0.2581284 -0.9162877 -0.3065935 0.2567667 -0.916554 -0.4726419 0.2588881 -0.8423696 0.05413478 0.2532923 -0.965874 0.05391412 0.2545722 -0.9655498 0.03948223 0.7060217 -0.7070888 -0.09333491 0.7052802 -0.7027577 -0.03303414 0.9680098 -0.2487279 -0.07829689 0.9681807 -0.2376884 0.06135183 0.9674766 -0.2454078 0.1740778 0.696309 -0.6963121 0.1739403 0.6986982 -0.6939493 0.2341818 0.2688215 -0.9342879 0.2314112 0.2534631 -0.9392579 0.5634199 0.2665578 -0.7819878 0.4170309 0.2669032 -0.8688198 0.4046592 0.2732187 -0.8726983 0.4044461 0.2603067 -0.8767347 0.2365246 0.2621741 -0.9355859 0.7046238 0.2473371 -0.6650787 0.7013934 0.2572429 -0.6647355 0.5154538 0.7040323 -0.488514 0.415661 0.7047259 -0.5749672 0.1455301 0.9686573 -0.2013059 0.15098 0.9678227 -0.2013067 0.2147524 0.9670686 -0.1366009 0.5968894 0.7068039 -0.3796725 0.6017773 0.7001448 -0.3842673 0.8154708 0.2526968 -0.5207222 0.8161497 0.2468252 -0.5224721 0.9629638 0.2696306 0 0.9449788 0.2718722 -0.1819358 0.9482918 0.2608299 -0.18086 0.9482879 0.261008 -0.1806234 0.8967889 0.2617092 -0.3567605 0.8974719 0.258296 -0.3575298 0.8130294 0.2577725 -0.5220504 0.9629638 0.2696306 0 0.9629638 0.2696306 0 0.7167264 0.6973544 0 0.70467 0.6965711 -0.1350152 0.2486808 0.9674128 -0.04764741 0.2481271 0.9675516 -0.04771661 0.9482879 0.261008 0.1806234 0.2469177 0.9678809 0.04730957 0.7014893 0.6998912 0.1344058 0.7047534 0.6966033 0.1344119 0.9482918 0.2608299 0.18086 0.9449788 0.2718721 0.1819358 0.2322279 0.9682156 0.09289127 0.6569463 0.7066604 0.2627788 0.6640112 0.699507 0.2641569 0.8976452 0.2582861 0.3571012 0.8966205 0.2616998 0.3571906 0.2123748 0.9678058 0.1350885 0.6024934 0.7000937 0.3832371 0.59627 0.7067469 0.3807505 0.8154708 0.2526969 0.5207222 0.8130294 0.2577725 0.5220504 0.5647161 0.2584066 0.7837867 0.7015674 0.2590524 0.6638488 0.7013934 0.2572429 0.6647355 0.7046238 0.2473371 0.6650787 0.8161497 0.2468252 0.5224721 0.4044461 0.2603067 0.8767347 0.4046592 0.2732187 0.8726983 0.3027438 0.6943063 0.6529051 0.4217932 0.6940296 0.5834496 0.1488554 0.9671839 0.2059057 0.1488695 0.9669557 0.2069647 0.06135171 0.9674767 0.2454074 0.1735082 0.6987219 0.6940336 0.1745008 0.6963297 0.6961856 0.2341818 0.2688215 0.9342879 0.2365246 0.2621741 0.9355859 -0.1291 0.2495277 0.9597235 0.05087167 0.2513077 0.9665695 0.05391412 0.2545722 0.9655498 0.05413478 0.2532923 0.965874 0.2314112 0.2534631 0.9392579 -0.3065935 0.2567667 0.916554 -0.3062461 0.2581284 0.9162877 -0.2280214 0.6946619 0.6822397 -0.09468263 0.6948394 0.7129049 -0.03354847 0.9669889 0.2526004 -0.03234249 0.9677872 0.2496837 -0.1255912 0.9665973 0.22342 -0.3491178 0.7017115 0.6210619 -0.3474251 0.7069473 0.616053 -0.4731478 0.2687683 0.8389843 -0.4726419 0.2588881 0.8423696 -0.8506854 0.2602313 0.4567428 -0.7513393 0.2621915 0.6055946 -0.7490752 0.2696682 0.6051161 -0.7517282 0.2706233 0.6013882 -0.6221522 0.2713071 0.7343835 -0.6228598 0.2618451 0.7372128 -0.4767771 0.2609524 0.839397 -0.8554773 0.2506282 0.4531491 -0.8501825 0.2659004 0.454408 -0.6330834 0.6962109 0.3383722 -0.5582225 0.6964006 0.4510145 -0.1946848 0.9681715 0.1572951 -0.1992351 0.9668231 0.15987 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0.2576645 0 -0.9662344 -0.05074721 0 -0.9987115 -0.05074721 0 -0.9987115 -0.3301701 0 -0.9439214 -0.3301701 0 -0.9439214 0.2314149 -0.04166501 -0.9719626 0.2337992 -0.0328713 -0.9717291 -0.2353472 -0.01319801 0.9718218 0.2360719 0 -0.9717356 0.498471 0 -0.8669065 0.5025548 0.05025458 -0.8630836 0.4977039 -0.0518344 -0.8657968 0.5000953 0.04980254 -0.8645372 0.5044022 0 -0.8634689 0.7249996 0 -0.6887493 0.7241208 -0.006897449 -0.6896386 0.8897777 0 -0.4563943 0.8898923 -0.008304476 -0.4560952 0.8900858 0 -0.4557931 0.7240293 0 -0.6897691 -0.9833266 0 0.1818479 -0.9818185 -0.044227 0.1845983 0.9825241 -0.002599418 -0.1861173 0.9825702 0 -0.1858918 0.6556872 0 0.7550326 -0.6556872 0 0.7550326 -0.6515577 -0.04579263 0.7572157 -0.6516571 -0.04426085 0.7572213 -0.6510875 0 0.7590027 -0.8414006 0 0.540412 -0.8395078 -0.03620713 0.54214 -0.8394931 0 0.5433704 -0.9587494 0 0.2842525 -0.9582376 0.04984301 0.2815961 -0.9599216 0 0.2802686 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -0.9598937 0 -0.2803642 -0.9588532 0.03467482 -0.281777 -0.9590831 0 -0.2831244 -0.8426965 0 -0.5383889 0.8393291 -0.04166615 0.5420246 0.8395044 0 0.5433529 0.6522963 0 0.757964 0.6490944 0.0781871 0.7566791 0.6515577 0.04579263 0.7572157 0.8899229 0 0.4561109 0.9838697 0 0.1788867 0.9821668 0.01870387 0.187079 0.9826549 -0.003704786 0.1854068 0.9822553 -0.03267395 0.1846804 0.9842713 0 0.1766641 0.8899229 0 0.4561109 0.7249996 0 0.6887493 0.7241208 -0.006897449 0.6896386 0.7232667 0.02902907 0.6899583 0.7348049 0 0.6782786 0.5044022 0 0.8634689 0.4990119 0.0604375 0.864485 0.5005733 0.02391499 0.8653637 0.498471 0 0.8669065 0.2360719 0 0.9717356 0.2353472 0.01319825 0.9718218 0.2337992 -0.03287124 0.9717291 0.2314148 -0.04166501 0.9719625 0.2576645 0 0.9662344 -0.05074721 0 0.9987115 -0.05074721 0 0.9987115 -0.3301701 0 0.9439214 0.3301701 0 -0.9439214 0 -1 0 0 -1 0 0 -1 0 0 0.03485059 -0.9993925 0.9993925 0.03485059 0 1 0 0 0 0 -1 -0.260666 0 0.9654291 -0.9654291 0 -0.260666 -0.9654291 0 -0.260666 -0.260666 0 -0.9654291 -0.9645484 0.04270619 -0.2604277 -0.9646556 0.04270696 -0.2600302 0.9654291 0 -0.260666 0.2604277 0.04270619 -0.9645484 0.7071068 0 0.7071068 0.2689354 0.9604846 -0.07171601 0.2758985 0.9581687 -0.07611 0.2031767 0.95783 -0.2031767 0.2031767 0.95783 -0.2031767 0.07389479 0.9579936 -0.2771059 0.07417577 0.9603113 -0.2688869 0.6940287 0.6940234 -0.1914564 0.6821739 0.707958 -0.1828504 0.4946855 0.7145437 -0.4946855 0.4946855 0.7145437 -0.4946855 0.1876068 0.7087336 -0.6800739 0.1862189 0.6947355 -0.694741 0.9324051 0.2610736 -0.2499225 0.9336825 0.2543331 -0.2520944 0.6852533 0.2466892 -0.6852533 0.6852534 0.2466892 -0.6852534 0.2503941 0.2542446 -0.9341641 0.2516342 0.2609541 -0.9319781 0 0 0 0 0.7071041 -0.7071095 0 0.7071041 -0.7071095 0 0.2696301 -0.9629639 0 0.2696301 -0.9629639 0.2696297 0.9629641 0 0.2696297 0.9629641 0 0.2696297 0.9629641 0 0.7071095 0.7071041 0 0.7071095 0.7071041 0 0.9629639 0.2696301 0 0.9629639 0.2696301 0 -0.07611 0.9581687 -0.2758985 -0.2031767 0.95783 -0.2031767 -0.2031767 0.95783 -0.2031767 -0.2771059 0.9579936 -0.07389479 -0.2688869 0.9603113 -0.07417577 -0.1914564 0.6940234 -0.6940287 -0.1828504 0.7079579 -0.6821739 -0.4946855 0.7145437 -0.4946855 -0.4946855 0.7145437 -0.4946855 -0.6800737 0.7087335 -0.1876068 -0.694741 0.6947355 -0.1862189 -0.2499225 0.2610736 -0.9324051 -0.2520944 0.2543331 -0.9336825 -0.6852533 0.2466892 -0.6852533 -0.6852534 0.2466892 -0.6852534 -0.9341641 0.2542446 -0.2503941 -0.9319781 0.2609541 -0.2516342 0.07171601 0.9604846 0.2689354 0.07611 0.9581687 0.2758985 0.2031767 0.95783 0.2031767 0.2031767 0.95783 0.2031767 0.2771059 0.9579936 0.07389479 0.2688869 0.9603113 0.07417577 0.1914564 0.6940234 0.6940287 0.1828504 0.7079579 0.6821739 0.4946855 0.7145437 0.4946855 0.4946855 0.7145437 0.4946855 0.6800737 0.7087335 0.1876068 0.694741 0.6947355 0.1862189 0.2499225 0.2610736 0.9324051 0.2520944 0.2543331 0.9336825 0.6852533 0.2466892 0.6852533 0.6852534 0.2466892 0.6852534 0.9341641 0.2542446 0.2503941 0.9319781 0.2609541 0.2516342 -0.2696297 0.9629641 0 -0.2696297 0.9629641 0 -0.2696297 0.9629641 0 -0.7071095 0.7071041 0 -0.7071095 0.7071041 0 -0.9629639 0.2696301 0 -0.9629639 0.2696301 0 0 0.9629641 0.2696297 0 0.9629641 0.2696297 0 0.9629641 0.2696297 0 0.7071041 0.7071095 0 0.7071041 0.7071095 0 0.2696301 0.9629639 0 0.2696301 0.9629639 -0.2689354 0.9604846 0.07171601 -0.2758985 0.9581687 0.07611 -0.2031767 0.95783 0.2031767 -0.2031767 0.95783 0.2031767 -0.07389479 0.9579936 0.2771059 -0.07417577 0.9603113 0.2688869 -0.6940287 0.6940234 0.1914564 -0.6821739 0.707958 0.1828504 -0.4946855 0.7145437 0.4946855 -0.4946855 0.7145437 0.4946855 -0.1876068 0.7087336 0.6800739 -0.1862189 0.6947355 0.694741 -0.9324051 0.2610736 0.2499225 -0.9336825 0.2543331 0.2520944 -0.6852533 0.2466892 0.6852533 -0.6852534 0.2466892 0.6852534 -0.2503941 0.2542446 0.9341641 -0.2516342 0.2609541 0.9319781 -0.9629639 0 -0.2696301 -0.9629639 0 -0.2696301 -0.7167251 0 -0.6973558 -0.7167237 -2.52567e-7 -0.6973572 -0.2516344 0 -0.9678224 -0.2516335 0 -0.9678226 -0.2516344 0 -0.9678224 -0.2516344 0 0.9678224 -0.2516344 0 0.9678224 -0.7167251 0 0.6973558 -0.7167251 0 0.6973558 -0.9629639 0 0.2696301 -0.9629639 0 0.2696301 -1 0 0 -1 0 0 0 0 1 0 0 -1 0 0 -1 -1 0 0 -1 0 0 -0.9122474 0 0.4096398 -0.8835567 -0.01679629 -0.4680228 -0.957763 0.01149314 -0.2873293 -0.957763 0.01149314 0.2873293 -0.8835567 -0.01679629 0.4680228 -0.9086417 0.007449328 0.4175103 -0.881016 0.007450222 0.4730277 -0.8049004 -0.01266139 0.5932749 -0.9122221 0.007446944 -0.4096284 -0.901296 0 -0.4332039 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 -0.9190827 0.28875 -0.2681613 -0.9068856 0.3034356 -0.292379 -0.9189138 0.2091057 0.3344731 0.9175001 0.1444488 -0.3705781 0.8302804 0.1654177 -0.5322325 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -0.2516344 0 0.9678224 0.002348303 0.6348381 0.7726417 -0.7167251 0 0.6973558 -0.7167251 0 0.6973558 -0.2516344 0 0.9678224 -0.2516344 0 0.9678224 -0.2516344 0 -0.9678224 -0.2516344 0 -0.9678224 -0.7167251 0 -0.6973558 -0.004324674 0.7071006 -0.7070997 0.003669202 0.9127396 -0.4085254 0.2344187 0.967514 -0.09468168 -0.7256583 0.6450302 -0.2394912 -0.3975354 0.9086532 0.1277299 0.2290577 0.9690912 -0.09162318 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 0 0 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 0 0 0 0 0 0.03485059 -0.9993925 0.9993925 0.03485059 0 -0.9993925 0.03485059 0 -0.9993925 0.03485059 0 0 0 1 0 0 1 1 0 0 0 0 -1 -1 0 0 -1 0 0 -0.7071068 0 0.7071068 -0.7071068 0 0.7071068 -0.260666 0 0.9654291 -0.9654291 0 0.260666 -0.9654291 0 0.260666 -0.9646556 0.04270696 0.2600302 -0.9645484 0.04270619 0.2604277 -0.706249 0.04924374 0.7062489 -0.7062489 0.04924374 0.7062489 -0.2604277 0.04270619 0.9645484 -0.2600302 0.04270696 0.9646556 -0.7071068 0 -0.7071068 -0.7071068 0 -0.7071068 -0.260666 0 -0.9654291 -0.2600301 0.04270696 -0.9646555 -0.2604277 0.04270619 -0.9645484 -0.7062489 0.04924374 -0.706249 -0.7062489 0.04924374 -0.7062489 0.260666 0 -0.9654291 0.7071068 0 -0.7071068 0.7071068 0 -0.7071068 0.260666 0 -0.9654291 0.9654291 0 -0.260666 0.9646556 0.04270696 -0.2600302 0.9645484 0.04270619 -0.2604277 0.706249 0.04924374 -0.7062489 0.7062489 0.04924374 -0.7062489 0.2600302 0.04270696 -0.9646556 0.2604278 0.04270619 0.9645484 0.2600301 0.04270696 0.9646556 0.9645479 0.04271709 0.2604277 0.964656 0.04269605 0.2600303 0.7062489 0.04924374 0.7062489 0.706249 0.04924374 0.7062489 0.7071068 0 0.7071068 0.9654291 0 0.260666 0.9654291 0 0.260666 0.260666 0 0.9654291 0.260666 0 0.9654291 0 0 0 0 0 0 -1.5404e-7 1 -6.36081e-7 3.24032e-7 1 5.61656e-7 6.54708e-7 1 0 -6.48629e-7 1 0 -5.82531e-7 1 2.98564e-7 4.26935e-7 1 4.96095e-7 -3.27791e-7 1 5.66667e-7 6.28108e-7 1 1.84581e-7 -1.5404e-7 1 6.36081e-7 5.49908e-7 1 -3.55121e-7 5.49908e-7 1 3.55121e-7 -4.74061e-7 1 -4.51486e-7 0 1 6.53629e-7 2.16121e-7 1 -6.17868e-7 3.24032e-7 1 -5.61656e-7 -5.82531e-7 1 -2.98564e-7 -4.74061e-7 1 4.51486e-7 4.26935e-7 1 -4.96095e-7 -3.27791e-7 1 -5.66667e-7 -6.43363e-7 1 1.20963e-7 2.16121e-7 1 6.17868e-7 6.28108e-7 1 -1.84581e-7 -6.43363e-7 1 -1.20963e-7 0 1 -6.53629e-7 0 0.9629641 -0.2696297 0 0.9629641 -0.2696297 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.1872387 -0.9823144 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.1364855 -0.9817008 -0.1327975 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.08765232 -0.9373711 -0.3371237 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.1734797 -0.9848374 0 0 0 0 0 0 0 0 0 0 -0.1734797 -0.9848374 0 0 0 0 0 0 0 0 0.9629641 -0.2696297 0 0 0 -0.07171601 0.9604846 -0.2689354 -0.08764123 -0.9899891 0.1106374 0.07545632 -0.9970984 0.01005166 -0.165484 -0.9851234 -0.04633498 0.06710433 -0.9963871 -0.05205571 -0.01855289 -0.9915714 -0.1282264 -0.1596735 -0.9861569 0.0447086 0.01166564 -0.9952132 0.09702873 0.1506127 0.98234 0.1110134 0.01357448 -0.9934884 -0.1131218 0 0 0 -0.1199531 -0.9858954 0.1167114 0.0334047 -0.9960209 0.08262425 0.07395762 -0.9967855 -0.03080356 -0.1550459 -0.986953 0.04341232 -0.04902708 -0.9923275 0.1135011 -0.05939245 -0.9717465 0.2284322 0.01359909 -0.9934894 -0.1131098 0 0 0 -0.1297165 -0.9869306 0.09561127 0.05006933 -0.9965522 0.06615769 0.07658731 -0.9970107 -0.01020228 -0.1044685 -0.9857454 -0.1318801 0.1044685 0.9857454 0.1318801 0 -0.9662557 0.2575849 -0.01562213 -0.9940313 0.1079711 -0.1443563 -0.9795069 -0.1404551 0.03801149 -0.9948446 -0.09401881 0.06233876 -0.9968829 0.04835891 -0.05876457 -0.9889584 -0.136044 -0.05089199 -0.9793348 0.195738 0.01164436 -0.9952126 0.09703755 -0.1872387 -0.9823144 0 0.05545455 -0.995769 -0.0732733 -0.1144365 -0.9871711 0.1113438 0.07075601 -0.9970582 0.02947002 -0.1506127 -0.98234 -0.1110134 0.05876457 0.9889584 0.136044 -0.1603716 -0.9844262 0.07201397 -0.01855289 -0.9915714 -0.1282265 0 0.03485065 0.9993925 0 0.03485059 0.9993926 0 0.03485053 0.9993925 0 0.03485059 0.9993926 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -0.1707559 -0.9841526 -0.04781168 0.070692 0.9597275 0.2718927 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.1790391 0.9805517 0.08039653 -0.1790391 -0.9805517 -0.08039653 0 -0.9156407 -0.4019977 + + + + + + + + + + + + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 3 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 3 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 3 4 4 4 4 4 4 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

376 0 371 0 375 0 325 1 326 1 329 1 161 2 75 2 120 2 122 3 76 3 284 3 277 4 404 4 278 4 278 5 404 5 280 5 280 6 404 6 406 6 280 7 406 7 282 7 408 8 275 8 406 8 406 9 275 9 281 9 406 10 281 10 282 10 410 11 259 11 408 11 408 12 259 12 266 12 408 13 266 13 275 13 414 14 263 14 264 14 414 15 264 15 412 15 412 16 264 16 262 16 412 17 262 17 410 17 410 18 262 18 260 18 410 19 260 19 259 19 263 20 414 20 257 20 257 21 414 21 416 21 257 22 416 22 248 22 248 23 416 23 241 23 241 24 416 24 418 24 241 25 418 25 242 25 420 26 246 26 418 26 418 27 246 27 244 27 418 28 244 28 242 28 230 29 234 29 423 29 423 30 234 30 239 30 423 31 239 31 420 31 420 32 239 32 245 32 420 33 245 33 246 33 230 34 423 34 229 34 229 35 423 35 422 35 229 36 422 36 221 36 221 37 422 37 214 37 214 38 422 38 421 38 214 39 421 39 215 39 215 40 421 40 217 40 217 41 421 41 419 41 217 42 419 42 219 42 417 43 212 43 419 43 419 44 212 44 218 44 419 45 218 45 219 45 199 46 197 46 415 46 415 47 197 47 196 47 415 48 196 48 417 48 417 49 196 49 203 49 417 50 203 50 212 50 199 51 415 51 201 51 201 52 415 52 413 52 201 53 413 53 200 53 200 54 413 54 194 54 194 55 413 55 411 55 194 56 411 56 185 56 185 57 411 57 409 57 185 58 409 58 178 58 178 59 409 59 407 59 178 60 407 60 179 60 405 61 183 61 407 61 407 62 183 62 181 62 407 63 181 63 179 63 167 64 171 64 403 64 403 65 171 65 176 65 403 66 176 66 405 66 405 67 176 67 182 67 405 68 182 68 183 68 167 69 403 69 166 69 166 70 403 70 402 70 166 71 402 71 162 71 84 72 76 72 122 72 122 73 123 73 84 73 84 74 123 74 150 74 84 75 150 75 151 75 151 76 124 76 84 76 126 77 125 77 84 77 84 78 125 78 27 78 84 79 27 79 82 79 118 80 117 80 107 80 107 81 106 81 28 81 120 82 107 82 121 82 121 83 107 83 119 83 118 84 107 84 130 84 130 85 107 85 28 85 130 86 28 86 129 86 41 87 40 87 127 87 31 88 30 88 173 88 114 89 74 89 36 89 28 90 106 90 34 90 108 91 35 91 110 91 110 92 35 92 34 92 110 93 34 93 109 93 109 94 34 94 106 94 108 95 111 95 35 95 36 96 116 96 114 96 78 97 40 97 80 97 80 98 40 98 79 98 40 99 81 99 83 99 125 100 127 100 27 100 27 101 127 101 40 101 27 102 40 102 82 102 82 103 40 103 83 103 79 104 40 104 77 104 77 105 40 105 39 105 77 106 39 106 73 106 73 107 39 107 38 107 73 108 38 108 74 108 74 109 38 109 37 109 74 110 37 110 36 110 173 111 30 111 152 111 152 112 30 112 29 112 152 113 29 113 188 113 188 114 29 114 71 114 188 115 71 115 190 115 190 116 71 116 72 116 190 117 72 117 134 117 173 118 133 118 31 118 31 119 133 119 169 119 31 120 169 120 32 120 32 121 169 121 164 121 32 122 164 122 33 122 33 123 164 123 132 123 33 124 132 124 34 124 34 125 132 125 159 125 34 126 159 126 28 126 28 127 159 127 131 127 28 128 131 128 129 128 127 129 128 129 41 129 41 130 128 130 148 130 41 131 148 131 42 131 42 132 148 132 291 132 42 133 291 133 43 133 43 134 291 134 289 134 43 135 289 135 44 135 44 136 289 136 158 136 44 137 158 137 45 137 158 138 157 138 45 138 45 139 157 139 272 139 45 140 272 140 46 140 46 141 272 141 146 141 46 142 146 142 47 142 47 143 146 143 147 143 47 144 147 144 48 144 48 145 147 145 271 145 48 146 271 146 49 146 49 147 271 147 269 147 49 148 269 148 50 148 269 149 156 149 50 149 50 150 156 150 155 150 50 151 155 151 51 151 51 152 155 152 254 152 51 153 254 153 52 153 52 154 254 154 144 154 52 155 144 155 53 155 53 156 144 156 145 156 53 157 145 157 54 157 145 158 253 158 54 158 54 159 253 159 251 159 54 160 251 160 55 160 55 161 251 161 154 161 55 162 154 162 56 162 56 163 154 163 236 163 56 164 236 164 57 164 57 165 236 165 143 165 57 166 143 166 58 166 143 167 232 167 58 167 58 168 232 168 142 168 58 169 142 169 59 169 59 170 142 170 227 170 59 171 227 171 60 171 60 172 227 172 140 172 60 173 140 173 61 173 61 174 140 174 141 174 61 175 141 175 62 175 141 176 226 176 62 176 62 177 226 177 224 177 62 178 224 178 63 178 64 179 209 179 139 179 64 180 139 180 65 180 65 181 139 181 138 181 65 182 138 182 66 182 66 183 138 183 137 183 66 184 137 184 67 184 67 185 137 185 208 185 67 186 208 186 68 186 208 187 206 187 68 187 68 188 206 188 153 188 68 189 153 189 69 189 69 190 153 190 191 190 69 191 191 191 70 191 70 192 191 192 136 192 70 193 136 193 72 193 72 194 136 194 135 194 72 195 135 195 134 195 71 196 308 196 309 196 71 197 29 197 308 197 308 198 29 198 30 198 308 199 30 199 292 199 30 200 31 200 292 200 292 201 31 201 32 201 292 202 32 202 307 202 32 203 33 203 307 203 307 204 33 204 34 204 307 205 34 205 306 205 34 206 35 206 306 206 306 207 35 207 36 207 306 208 36 208 305 208 36 209 37 209 305 209 305 210 37 210 38 210 305 211 38 211 304 211 38 212 39 212 304 212 304 213 39 213 40 213 304 214 40 214 303 214 40 215 41 215 303 215 303 216 41 216 42 216 303 217 42 217 302 217 42 218 43 218 302 218 302 219 43 219 44 219 302 220 44 220 301 220 44 221 45 221 301 221 301 222 45 222 46 222 301 223 46 223 293 223 46 224 47 224 293 224 293 225 47 225 48 225 293 226 48 226 294 226 48 227 49 227 294 227 294 228 49 228 50 228 294 229 50 229 295 229 50 230 51 230 295 230 295 231 51 231 52 231 295 232 52 232 296 232 52 233 53 233 296 233 296 234 53 234 54 234 296 235 54 235 297 235 54 236 55 236 297 236 297 237 55 237 56 237 297 238 56 238 298 238 56 239 57 239 298 239 298 240 57 240 58 240 298 241 58 241 299 241 58 242 59 242 299 242 299 243 59 243 60 243 299 244 60 244 300 244 60 245 61 245 300 245 300 246 61 246 62 246 300 247 62 247 313 247 62 248 63 248 313 248 313 249 63 249 64 249 313 250 64 250 312 250 64 251 65 251 312 251 312 252 65 252 66 252 312 253 66 253 311 253 66 254 67 254 311 254 311 255 67 255 68 255 311 256 68 256 310 256 68 257 69 257 310 257 310 258 69 258 70 258 310 259 70 259 309 259 309 260 70 260 72 260 309 261 72 261 71 261 399 262 95 262 398 262 398 263 95 263 99 263 398 264 99 264 397 264 93 265 77 265 73 265 93 266 73 266 115 266 115 267 73 267 74 267 115 268 74 268 114 268 77 269 93 269 79 269 79 270 93 270 89 270 85 271 78 271 89 271 89 272 78 272 80 272 89 273 80 273 79 273 81 274 85 274 83 274 83 275 85 275 84 275 83 276 84 276 82 276 95 277 399 277 401 277 95 278 401 278 92 278 84 279 87 279 88 279 86 280 87 280 84 280 85 281 86 281 84 281 85 282 89 282 86 282 86 283 89 283 90 283 86 284 90 284 87 284 87 285 90 285 91 285 87 286 91 286 88 286 88 287 91 287 92 287 89 288 93 288 90 288 90 289 93 289 94 289 90 290 94 290 91 290 91 291 94 291 96 291 91 292 96 292 92 292 92 293 96 293 95 293 93 294 115 294 97 294 93 295 97 295 94 295 94 296 97 296 98 296 94 297 98 297 96 297 96 298 98 298 99 298 96 299 99 299 95 299 105 300 400 300 103 300 103 301 400 301 397 301 103 302 397 302 99 302 101 303 112 303 107 303 101 304 107 304 104 304 115 305 113 305 97 305 97 306 113 306 100 306 97 307 100 307 98 307 98 308 100 308 102 308 98 309 102 309 99 309 99 310 102 310 103 310 113 311 112 311 100 311 100 312 112 312 101 312 100 313 101 313 102 313 102 314 101 314 104 314 102 315 104 315 103 315 103 316 104 316 105 316 105 317 104 317 107 317 106 318 107 318 109 318 109 319 107 319 112 319 111 320 108 320 112 320 112 321 108 321 110 321 112 322 110 322 109 322 116 323 113 323 115 323 116 324 115 324 114 324 268 325 274 325 276 325 250 326 256 326 258 326 223 327 228 327 231 327 205 328 211 328 213 328 187 329 193 329 195 329 158 330 289 330 290 330 272 331 157 331 273 331 156 332 269 332 270 332 254 333 155 333 255 333 154 334 251 334 252 334 153 335 206 335 207 335 173 336 152 336 174 336 164 337 169 337 170 337 163 338 117 338 160 338 160 339 117 339 118 339 160 340 118 340 130 340 119 341 163 341 121 341 121 342 163 342 161 342 121 343 161 343 120 343 122 344 284 344 123 344 123 345 284 345 286 345 123 346 286 346 150 346 149 347 124 347 151 347 125 348 126 348 127 348 127 349 126 349 149 349 127 350 149 350 128 350 128 351 149 351 148 351 159 352 160 352 131 352 131 353 160 353 130 353 131 354 130 354 129 354 159 355 132 355 165 355 165 356 132 356 164 356 169 357 133 357 175 357 175 358 133 358 173 358 190 359 134 359 187 359 187 360 134 360 135 360 187 361 135 361 193 361 193 362 135 362 136 362 193 363 136 363 191 363 208 364 137 364 205 364 205 365 137 365 138 365 205 366 138 366 211 366 211 367 138 367 139 367 211 368 139 368 209 368 227 369 228 369 140 369 140 370 228 370 223 370 140 371 223 371 141 371 141 372 223 372 226 372 227 373 142 373 233 373 233 374 142 374 232 374 232 375 143 375 238 375 238 376 143 376 236 376 254 377 256 377 144 377 144 378 256 378 250 378 144 379 250 379 145 379 145 380 250 380 253 380 272 381 274 381 146 381 146 382 274 382 268 382 146 383 268 383 147 383 147 384 268 384 271 384 291 385 148 385 288 385 288 386 148 386 149 386 288 387 149 387 286 387 286 388 149 388 151 388 286 389 151 389 150 389 180 390 184 390 174 390 152 391 188 391 174 391 174 392 188 392 189 392 174 393 189 393 180 393 180 394 189 394 186 394 180 395 186 395 178 395 178 396 186 396 185 396 198 397 202 397 192 397 191 398 153 398 192 398 192 399 153 399 207 399 192 400 207 400 198 400 198 401 207 401 204 401 198 402 204 402 196 402 196 403 204 403 203 403 216 404 220 404 210 404 210 405 225 405 216 405 216 406 225 406 222 406 243 407 247 407 237 407 236 408 154 408 237 408 237 409 154 409 252 409 237 410 252 410 243 410 243 411 252 411 249 411 243 412 249 412 241 412 241 413 249 413 248 413 261 414 265 414 255 414 155 415 156 415 255 415 255 416 156 416 270 416 255 417 270 417 261 417 261 418 270 418 267 418 261 419 267 419 259 419 259 420 267 420 266 420 279 421 283 421 273 421 157 422 158 422 273 422 273 423 158 423 290 423 273 424 290 424 279 424 279 425 290 425 287 425 168 426 166 426 162 426 159 427 165 427 160 427 160 428 165 428 168 428 160 429 168 429 163 429 163 430 168 430 162 430 163 431 162 431 161 431 172 432 171 432 167 432 164 433 170 433 165 433 165 434 170 434 172 434 165 435 172 435 168 435 168 436 172 436 167 436 168 437 167 437 166 437 169 438 175 438 170 438 170 439 175 439 177 439 170 440 177 440 172 440 172 441 177 441 176 441 172 442 176 442 171 442 173 443 174 443 175 443 175 444 174 444 184 444 175 445 184 445 177 445 177 446 184 446 182 446 177 447 182 447 176 447 178 448 179 448 180 448 180 449 179 449 181 449 180 450 181 450 184 450 184 451 181 451 183 451 184 452 183 452 182 452 194 453 185 453 195 453 195 454 185 454 186 454 195 455 186 455 187 455 187 456 186 456 189 456 187 457 189 457 190 457 190 458 189 458 188 458 191 459 192 459 193 459 193 460 192 460 202 460 193 461 202 461 195 461 195 462 202 462 200 462 195 463 200 463 194 463 196 464 197 464 198 464 198 465 197 465 199 465 198 466 199 466 202 466 202 467 199 467 201 467 202 468 201 468 200 468 212 469 203 469 213 469 213 470 203 470 204 470 213 471 204 471 205 471 205 472 204 472 207 472 205 473 207 473 208 473 208 474 207 474 206 474 209 475 210 475 211 475 211 476 210 476 220 476 211 477 220 477 213 477 213 478 220 478 218 478 213 479 218 479 212 479 221 480 214 480 222 480 222 481 214 481 215 481 222 482 215 482 216 482 216 483 215 483 217 483 216 484 217 484 220 484 220 485 217 485 219 485 220 486 219 486 218 486 229 487 221 487 231 487 231 488 221 488 222 488 231 489 222 489 223 489 223 490 222 490 225 490 223 491 225 491 226 491 226 492 225 492 224 492 235 493 234 493 230 493 227 494 233 494 228 494 228 495 233 495 235 495 228 496 235 496 231 496 231 497 235 497 230 497 231 498 230 498 229 498 232 499 238 499 233 499 233 500 238 500 240 500 233 501 240 501 235 501 235 502 240 502 239 502 235 503 239 503 234 503 236 504 237 504 238 504 238 505 237 505 247 505 238 506 247 506 240 506 240 507 247 507 245 507 240 508 245 508 239 508 241 509 242 509 243 509 243 510 242 510 244 510 243 511 244 511 247 511 247 512 244 512 246 512 247 513 246 513 245 513 257 514 248 514 258 514 258 515 248 515 249 515 258 516 249 516 250 516 250 517 249 517 252 517 250 518 252 518 253 518 253 519 252 519 251 519 254 520 255 520 256 520 256 521 255 521 265 521 256 522 265 522 258 522 258 523 265 523 263 523 258 524 263 524 257 524 259 525 260 525 261 525 261 526 260 526 262 526 261 527 262 527 265 527 265 528 262 528 264 528 265 529 264 529 263 529 275 530 266 530 276 530 276 531 266 531 267 531 276 532 267 532 268 532 268 533 267 533 270 533 268 534 270 534 271 534 271 535 270 535 269 535 272 536 273 536 274 536 274 537 273 537 283 537 274 538 283 538 276 538 276 539 283 539 281 539 276 540 281 540 275 540 285 541 277 541 287 541 287 542 277 542 278 542 287 543 278 543 279 543 279 544 278 544 280 544 279 545 280 545 283 545 283 546 280 546 282 546 283 547 282 547 281 547 284 548 285 548 286 548 286 549 285 549 287 549 286 550 287 550 288 550 288 551 287 551 290 551 288 552 290 552 291 552 291 553 290 553 289 553 301 554 293 554 389 554 308 555 292 555 363 555 363 556 292 556 307 556 389 557 293 557 385 557 385 558 293 558 294 558 385 559 294 559 380 559 380 560 294 560 295 560 380 561 295 561 377 561 377 562 295 562 296 562 377 563 296 563 373 563 373 564 296 564 297 564 373 565 297 565 370 565 370 566 297 566 298 566 370 567 298 567 390 567 390 568 298 568 299 568 390 569 299 569 393 569 393 570 299 570 300 570 393 571 300 571 313 571 389 572 387 572 301 572 301 573 387 573 365 573 301 574 365 574 302 574 302 575 365 575 342 575 302 576 342 576 303 576 303 577 342 577 345 577 303 578 345 578 304 578 304 579 345 579 351 579 304 580 351 580 305 580 305 581 351 581 354 581 305 582 354 582 306 582 306 583 354 583 357 583 306 584 357 584 307 584 307 585 357 585 359 585 307 586 359 586 363 586 363 587 337 587 308 587 308 588 337 588 315 588 308 589 315 589 309 589 309 590 315 590 319 590 309 591 319 591 310 591 310 592 319 592 324 592 310 593 324 593 311 593 311 594 324 594 327 594 311 595 327 595 312 595 312 596 327 596 330 596 312 597 330 597 313 597 313 598 330 598 334 598 313 599 334 599 393 599 317 600 314 600 319 600 314 601 315 601 319 601 314 602 316 602 315 602 316 603 337 603 315 603 316 604 336 604 337 604 317 605 319 605 318 605 319 606 320 606 318 606 319 607 320 607 324 607 324 608 321 608 320 608 324 609 322 609 321 609 322 610 324 610 323 610 324 611 325 611 323 611 324 612 327 612 325 612 325 613 327 613 326 613 327 614 329 614 326 614 327 615 330 615 329 615 333 616 328 616 334 616 328 617 330 617 334 617 328 618 331 618 330 618 331 619 329 619 330 619 392 620 393 620 332 620 332 621 393 621 334 621 332 622 335 622 334 622 335 623 333 623 334 623 361 624 338 624 363 624 364 625 365 625 339 625 365 626 340 626 339 626 365 627 342 627 340 627 340 628 342 628 341 628 342 629 343 629 341 629 342 630 345 630 343 630 343 631 345 631 344 631 345 632 346 632 344 632 345 633 351 633 346 633 346 634 351 634 347 634 351 635 348 635 347 635 351 636 349 636 348 636 349 637 351 637 350 637 351 638 352 638 350 638 351 639 354 639 352 639 352 640 354 640 353 640 354 641 355 641 353 641 354 642 357 642 355 642 355 643 357 643 356 643 357 644 358 644 356 644 357 645 358 645 359 645 359 646 358 646 360 646 359 647 360 647 363 647 363 648 360 648 362 648 363 649 362 649 361 649 372 650 370 650 373 650 390 651 366 651 391 651 366 652 390 652 367 652 390 653 368 653 367 653 390 654 370 654 368 654 370 655 369 655 368 655 370 656 372 656 369 656 375 657 371 657 377 657 371 658 373 658 377 658 371 659 374 659 373 659 374 660 372 660 373 660 375 661 377 661 376 661 377 662 378 662 376 662 377 663 380 663 378 663 378 664 380 664 379 664 380 665 381 665 379 665 380 666 385 666 381 666 385 667 382 667 381 667 385 668 383 668 382 668 383 669 385 669 384 669 385 670 386 670 384 670 385 671 389 671 386 671 389 672 388 672 386 672 389 673 388 673 387 673 559 708 557 708 593 708 595 708 6 787 26 787 3 787 6 788 22 788 26 788 8 789 22 789 6 789 8 790 20 790 22 790 11 791 20 791 8 791 11 792 18 792 20 792 25 793 0 793 2 793 1 794 24 794 4 794 9 795 19 795 10 795 7 796 19 796 9 796 7 797 21 797 19 797 5 798 21 798 7 798 5 799 23 799 21 799 23 800 4 800 24 800 5 801 4 801 23 801 10 802 15 802 12 802 18 803 13 803 14 803 11 804 13 804 18 804 3 805 25 805 2 805 26 806 25 806 3 806 16 807 15 807 17 807 162 808 13 808 161 808 161 809 13 809 75 809 284 810 76 810 16 810 284 811 16 811 285 811 285 812 16 812 17 812 285 813 17 813 277 813 277 814 17 814 404 814 162 815 402 815 14 815 162 816 14 816 13 816 4 817 76 817 84 817 84 818 124 818 126 818 107 819 117 819 119 819 75 820 2 820 120 820 120 821 2 821 107 821 35 822 111 822 36 822 36 823 111 823 116 823 78 824 81 824 40 824 63 825 224 825 209 825 63 826 209 826 64 826 75 827 13 827 11 827 2 828 75 828 3 828 3 829 75 829 11 829 3 830 11 830 6 830 6 831 11 831 8 831 12 832 15 832 16 832 16 833 76 833 12 833 12 834 76 834 4 834 12 835 4 835 9 835 9 836 4 836 5 836 9 837 5 837 7 837 9 838 10 838 12 838 85 839 81 839 78 839 92 840 401 840 1 840 92 841 1 841 88 841 88 842 1 842 4 842 88 843 4 843 84 843 105 844 107 844 2 844 2 845 0 845 105 845 105 846 0 846 400 846 111 847 112 847 113 847 111 848 113 848 116 848 209 849 224 849 225 849 163 850 119 850 117 850 126 851 124 851 149 851 210 852 209 852 225 852 550 859 548 859 407 859 409 859 544 860 543 860 402 860 403 860 545 868 534 868 570 868 581 868 423 869 420 869 561 869 564 869 590 919 588 919 552 919 554 919 558 920 556 920 415 920 417 920 530 921 385 921 380 921 530 922 337 922 363 922 530 923 354 923 351 923 530 924 390 924 393 924 530 925 334 925 330 925 530 926 363 926 359 926 530 927 327 927 324 927 530 928 357 928 354 928 530 929 324 929 319 929 530 930 345 930 342 930 530 931 359 931 357 931 530 932 377 932 373 932 530 933 319 933 315 933 389 934 530 934 387 934 530 935 365 935 387 935 530 936 373 936 370 936 530 937 330 937 327 937 365 938 530 938 342 938 530 939 380 939 377 939 530 940 393 940 334 940 337 941 530 941 315 941 530 942 351 942 345 942 390 943 530 943 370 943 530 944 389 944 385 944 395 947 574 947 538 947 531 947 414 948 412 948 553 948 555 948 1 949 401 949 542 949 531 949 536 950 535 950 19 950 21 950 560 951 558 951 417 951 419 951 541 952 539 952 397 952 400 952 25 953 26 953 644 953 535 954 532 954 10 954 19 954 476 955 477 955 567 955 566 955 422 956 423 956 564 956 563 956 406 957 404 957 545 957 547 957 552 958 550 958 409 958 411 958 416 959 414 959 555 959 557 959 531 960 538 960 24 960 1 960 537 961 536 961 21 961 23 961 562 962 560 962 419 962 421 962 401 963 399 963 540 963 542 963 408 964 406 964 547 964 549 964 554 965 552 965 411 965 413 965 400 966 0 966 479 966 541 966 533 967 534 967 17 967 15 967 418 968 416 968 557 968 559 968 23 969 24 969 538 969 537 969 475 970 476 970 566 970 565 970 421 971 422 971 563 971 562 971 546 972 544 972 403 972 405 972 404 973 17 973 534 973 545 973 410 974 408 974 549 974 551 974 556 975 554 975 413 975 415 975 532 976 533 976 15 976 10 976 420 977 418 977 559 977 561 977 409 978 475 978 565 978 550 978 477 979 478 979 568 979 567 979 548 980 546 980 405 980 407 980 412 981 410 981 551 981 553 981 587 982 585 982 624 982 626 982 605 983 608 983 569 983 394 983 602 984 603 984 642 984 641 984 621 985 619 985 580 985 582 985 598 986 599 986 638 986 637 986 595 987 593 987 632 987 634 987 581 988 570 988 609 988 620 988 573 989 572 989 536 989 537 989 562 990 563 990 599 990 598 990 582 991 580 991 544 991 546 991 566 992 567 992 603 992 602 992 394 993 569 993 533 993 532 993 551 994 549 994 585 994 587 994 396 995 577 995 541 995 479 995 592 996 590 996 554 996 556 996 561 997 559 997 595 997 597 997 537 998 538 998 574 998 573 998 584 999 582 999 546 999 548 999 571 1000 394 1000 532 1000 535 1000 553 1001 551 1001 587 1001 589 1001 594 1002 592 1002 556 1002 558 1002 564 1003 561 1003 597 1003 600 1003 580 1004 579 1004 543 1004 544 1004 565 1005 566 1005 602 1005 601 1005 569 1006 570 1006 534 1006 533 1006 586 1007 584 1007 548 1007 550 1007 555 1008 553 1008 589 1008 591 1008 550 1009 565 1009 601 1009 586 1009 596 1010 594 1010 558 1010 560 1010 577 1011 575 1011 539 1011 541 1011 644 1012 22 1012 20 1012 563 1013 564 1013 600 1013 599 1013 547 1014 545 1014 581 1014 583 1014 567 1015 568 1015 604 1015 603 1015 572 1016 571 1016 535 1016 536 1016 588 1017 586 1017 550 1017 552 1017 531 1018 542 1018 578 1018 395 1018 557 1019 555 1019 591 1019 593 1019 598 1020 596 1020 560 1020 562 1020 542 1021 540 1021 576 1021 578 1021 549 1022 547 1022 583 1022 585 1022 607 1023 616 1023 577 1023 396 1023 631 1024 629 1024 590 1024 592 1024 597 1025 595 1025 634 1025 636 1025 623 1026 621 1026 582 1026 584 1026 610 1027 605 1027 394 1027 571 1027 589 1028 587 1028 626 1028 628 1028 573 1029 574 1029 613 1029 612 1029 633 1030 631 1030 592 1030 594 1030 611 1031 610 1031 571 1031 572 1031 600 1032 597 1032 636 1032 639 1032 619 1033 618 1033 579 1033 580 1033 601 1034 602 1034 641 1034 640 1034 608 1035 609 1035 570 1035 569 1035 625 1036 623 1036 584 1036 586 1036 591 1037 589 1037 628 1037 630 1037 612 1038 611 1038 572 1038 573 1038 586 1039 601 1039 640 1039 625 1039 635 1040 633 1040 594 1040 596 1040 616 1041 614 1041 575 1041 577 1041 599 1042 600 1042 639 1042 638 1042 583 1043 581 1043 620 1043 622 1043 603 1044 604 1044 643 1044 642 1044 627 1045 625 1045 586 1045 588 1045 607 1046 25 1046 644 1046 395 1047 578 1047 617 1047 606 1047 593 1048 591 1048 630 1048 632 1048 637 1049 635 1049 596 1049 598 1049 578 1050 576 1050 615 1050 617 1050 585 1051 583 1051 622 1051 624 1051 606 1052 613 1052 574 1052 395 1052 629 1053 627 1053 588 1053 590 1053 396 1054 25 1054 607 1054 644 1055 615 1055 398 1055 398 1056 615 1056 576 1056 614 1057 398 1057 575 1057 398 1058 540 1058 399 1058 644 1059 398 1059 614 1059 398 1060 576 1060 540 1060 539 1061 398 1061 397 1061 575 1063 398 1063 539 1063 622 1065 620 1065 644 1065 638 1066 639 1066 644 1066 644 1067 614 1067 616 1067 644 1068 633 1068 635 1068 625 1069 644 1069 640 1069 644 1070 611 1070 612 1070 630 1071 628 1071 644 1071 642 1072 643 1072 644 1072 644 1073 625 1073 627 1073 479 1074 25 1074 396 1074 606 1075 617 1075 644 1075 632 1076 630 1076 644 1076 644 1077 635 1077 637 1077 617 1078 615 1078 644 1078 624 1079 622 1079 644 1079 644 1080 613 1080 606 1080 644 1081 627 1081 629 1081 0 1082 25 1082 479 1082 620 1083 609 1083 644 1083 634 1084 632 1084 644 1084 637 1085 638 1085 644 1085 644 1086 619 1086 621 1086 641 1087 642 1087 644 1087 644 1088 608 1088 605 1088 626 1089 624 1089 644 1089 644 1090 616 1090 607 1090 644 1091 629 1091 631 1091 636 1092 634 1092 644 1092 644 1093 621 1093 623 1093 644 1094 605 1094 610 1094 628 1095 626 1095 644 1095 612 1096 613 1096 644 1096 644 1097 631 1097 633 1097 644 1098 610 1098 611 1098 639 1099 636 1099 644 1099 644 1100 618 1100 619 1100 640 1101 641 1101 644 1101 644 1102 609 1102 608 1102 644 1103 623 1103 625 1103 26 1128 22 1128 644 1128 644 1129 18 1129 20 1129 543 1130 14 1130 402 1130 478 1131 14 1131 568 1131 568 1132 14 1132 604 1132 579 1133 14 1133 543 1133 604 1134 14 1134 643 1134 618 1135 14 1135 579 1135 643 1136 14 1136 644 1136 644 1137 14 1137 618 1137 18 1138 14 1138 644 1138

+ + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

453 674 526 674 528 674 453 675 503 675 524 675 453 676 449 676 503 676 503 677 449 677 466 677 529 678 528 678 520 678 523 679 521 679 522 679 483 680 481 680 468 680 467 681 469 681 457 681 430 682 432 682 439 682 439 683 441 683 430 683 466 684 456 684 468 684 426 685 424 685 441 685 441 686 424 686 430 686 520 687 514 687 522 687 497 688 524 688 503 688 498 689 515 689 500 689 505 690 516 690 501 690 501 691 516 691 508 691 501 692 508 692 491 692 491 693 508 693 493 693 491 694 493 694 489 694 489 695 493 695 487 695 516 696 518 696 508 696 508 697 518 697 510 697 508 698 510 698 493 698 493 699 510 699 495 699 493 700 495 700 487 700 487 701 495 701 485 701 518 702 522 702 510 702 510 703 522 703 514 703 510 704 514 704 495 704 495 705 514 705 499 705 495 706 499 706 485 706 485 707 499 707 483 707 472 709 487 709 485 709 472 710 485 710 470 710 470 711 485 711 483 711 470 712 483 712 468 712 506 713 507 713 517 713 516 714 505 714 506 714 506 715 517 715 516 715 516 716 517 716 519 716 516 717 519 717 518 717 518 718 519 718 523 718 518 719 523 719 522 719 464 720 472 720 460 720 464 721 460 721 451 721 451 722 460 722 444 722 451 723 444 723 446 723 446 724 444 724 436 724 472 725 470 725 460 725 460 726 470 726 458 726 460 727 458 727 444 727 444 728 458 728 442 728 444 729 442 729 436 729 436 730 442 730 434 730 470 731 468 731 458 731 458 732 468 732 456 732 458 733 456 733 442 733 442 734 456 734 439 734 442 735 439 735 434 735 434 736 439 736 432 736 490 737 488 737 492 737 492 738 488 738 494 738 492 739 494 739 502 739 502 740 494 740 509 740 502 741 509 741 507 741 507 742 509 742 517 742 488 743 486 743 494 743 494 744 486 744 496 744 494 745 496 745 509 745 509 746 496 746 511 746 509 747 511 747 517 747 517 748 511 748 519 748 486 749 484 749 496 749 496 750 484 750 500 750 496 751 500 751 511 751 511 752 500 752 515 752 511 753 515 753 519 753 519 754 515 754 523 754 447 755 446 755 436 755 437 756 448 756 447 756 447 757 436 757 437 757 437 758 436 758 434 758 437 759 434 759 435 759 435 760 434 760 432 760 435 761 432 761 433 761 480 762 474 762 473 762 488 763 490 763 480 763 480 764 473 764 488 764 488 765 473 765 471 765 488 766 471 766 486 766 486 767 471 767 469 767 486 768 469 768 484 768 448 769 437 769 452 769 452 770 437 770 445 770 452 771 445 771 465 771 465 772 445 772 461 772 465 773 461 773 474 773 474 774 461 774 473 774 437 775 435 775 445 775 445 776 435 776 443 776 445 777 443 777 461 777 461 778 443 778 459 778 461 779 459 779 473 779 473 780 459 780 471 780 435 781 433 781 443 781 443 782 433 782 440 782 443 783 440 783 459 783 459 784 440 784 457 784 459 785 457 785 471 785 471 786 457 786 469 786 529 853 527 853 453 853 450 854 429 854 453 854 528 855 529 855 453 855 429 856 427 856 453 856 525 857 504 857 453 857 504 858 450 858 453 858 453 861 425 861 424 861 453 862 428 862 449 862 453 863 426 863 428 863 453 864 424 864 426 864 527 865 525 865 453 865 453 866 524 866 526 866 427 867 425 867 453 867 503 870 466 870 481 870 529 871 520 871 521 871 424 872 425 872 431 872 424 873 431 873 430 873 469 874 467 874 484 874 484 875 467 875 482 875 522 876 521 876 520 876 468 877 481 877 466 877 432 878 430 878 433 878 433 879 430 879 431 879 440 880 438 880 457 880 457 881 438 881 454 881 457 882 454 882 467 882 438 883 440 883 431 883 431 884 440 884 433 884 438 885 431 885 425 885 438 886 425 886 427 886 438 887 427 887 454 887 454 888 427 888 429 888 454 889 429 889 450 889 454 890 450 890 467 890 456 891 455 891 439 891 439 892 455 892 441 892 455 893 456 893 466 893 455 894 466 894 449 894 455 895 449 895 428 895 455 896 428 896 441 896 441 897 428 897 426 897 481 898 483 898 499 898 514 899 512 899 499 899 499 900 512 900 497 900 499 901 497 901 481 901 512 902 514 902 520 902 512 903 520 903 528 903 512 904 528 904 526 904 512 905 526 905 497 905 497 906 526 906 524 906 497 907 503 907 481 907 525 908 498 908 504 908 504 909 498 909 482 909 529 910 521 910 527 910 527 911 521 911 513 911 498 912 525 912 527 912 498 913 527 913 513 913 498 914 513 914 515 914 515 915 513 915 523 915 523 916 513 916 521 916 498 917 500 917 482 917 482 918 500 918 484 918 462 945 489 945 487 945 462 946 487 946 472 946 472 1062 463 1062 462 1062 463 1064 472 1064 464 1064 645 1104 450 1104 504 1104 504 1105 482 1105 645 1105 482 1106 467 1106 645 1106 467 1107 450 1107 645 1107 646 1108 452 1108 465 1108 489 1109 462 1109 646 1109 646 1110 506 1110 505 1110 646 1111 465 1111 474 1111 463 1112 464 1112 646 1112 447 1113 448 1113 646 1113 646 1114 492 1114 502 1114 646 1115 490 1115 492 1115 646 1116 502 1116 507 1116 646 1117 448 1117 452 1117 646 1118 507 1118 506 1118 446 1119 447 1119 646 1119 462 1120 463 1120 646 1120 501 1121 491 1121 646 1121 505 1122 501 1122 646 1122 451 1123 446 1123 646 1123 646 1124 480 1124 490 1124 646 1125 474 1125 480 1125 491 1126 489 1126 646 1126 464 1127 451 1127 646 1127

+ + + 1 + + + + + + 7.481132 -6.50764 5.343665 + 0 0 1 46.69194 + 0 1 0 0.619768 + 1 0 0 63.5593 + 1 1 1 + + + + 4.076245 1.005454 5.903862 + 0 0 1 106.9363 + 0 1 0 3.163707 + 1 0 0 37.26105 + 1 1 1 + + + + 0 0 0 + 0 0 1 0 + 0 1 0 0 + 1 0 0 90.00001 + 1 1 1 + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/plc_bringup/package.xml b/plc_bringup/package.xml new file mode 100644 index 0000000..020f038 --- /dev/null +++ b/plc_bringup/package.xml @@ -0,0 +1,33 @@ + + + + plc_bringup + 1.1.13 + Bringup scripts and configurations for the Nav2 stack + Michael Jeronimo + Steve Macenski + Carlos Orduno + Apache-2.0 + + ament_cmake + nav2_common + + navigation2 + launch_ros + + launch_ros + navigation2 + nav2_common + slam_toolbox + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest + launch + launch_testing + + + ament_cmake + + diff --git a/plc_bringup/params/nav2_multirobot_params_1.yaml b/plc_bringup/params/nav2_multirobot_params_1.yaml new file mode 100644 index 0000000..58d12bc --- /dev/null +++ b/plc_bringup/params/nav2_multirobot_params_1.yaml @@ -0,0 +1,296 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: True + global_frame: odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /robot1/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: True + robot_radius: 0.22 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /robot1/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + save_map_timeout: 5.0 + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/plc_bringup/params/nav2_multirobot_params_2.yaml b/plc_bringup/params/nav2_multirobot_params_2.yaml new file mode 100644 index 0000000..f1704ed --- /dev/null +++ b/plc_bringup/params/nav2_multirobot_params_2.yaml @@ -0,0 +1,295 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: True + global_frame: odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /robot2/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: True + robot_radius: 0.22 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /robot2/scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + save_map_timeout: 5.0 + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/plc_bringup/params/nav2_multirobot_params_all.yaml b/plc_bringup/params/nav2_multirobot_params_all.yaml new file mode 100644 index 0000000..372dcdd --- /dev/null +++ b/plc_bringup/params/nav2_multirobot_params_all.yaml @@ -0,0 +1,345 @@ +amcl: + ros__parameters: + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + # '' keyword shall be replaced with 'namespace' where user defined. + # It doesn't need to start with '/' + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + # '' keyword shall be replaced with 'namespace' where user defined. + # It doesn't need to start with '/' + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/plc_bringup/params/nav2_params.yaml b/plc_bringup/params/nav2_params.yaml new file mode 100644 index 0000000..8b076c6 --- /dev/null +++ b/plc_bringup/params/nav2_params.yaml @@ -0,0 +1,360 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::OmniMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: /scan + # set_initial_pose: false + # always_reset_initial_pose: false + # first_map_only: false + # initial_pose: + # x: 0.0 + # y: 0.0 + # z: 0.0 + # yaw: 0.0 + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # pure_pursuit parameters + FollowPath: + # plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + plugin: "omni_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 1.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.1 + max_lookahead_dist: 1.0 + lookahead_time: 0.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: true + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 1.0 + use_collision_detection: true + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.1 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + max_robot_pose_search_dist: 10.0 + use_interpolation: false + cost_scaling_dist: 0.3 + cost_scaling_gain: 1.0 + inflation_cost_scaling_factor: 3.0 + + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.1 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.50 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.1 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.50 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + # GridBased: + # plugin: "nav2_navfn_planner/NavfnPlanner" + # tolerance: 0.5 + # use_astar: true + # allow_unknown: true + GridBased: + plugin: "nav2_smac_planner/SmacPlanner2D" + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 2.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [1.0, 1.0, 1.0] + min_velocity: [-1.0, -1.0, -1.0] + max_accel: [2.5, 2.5, 3.2] + max_decel: [-2.5, -2.5, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/plc_bringup/rviz/nav2_default_view.rviz b/plc_bringup/rviz/nav2_default_view.rviz new file mode 100644 index 0000000..6cc9ce1 --- /dev/null +++ b/plc_bringup/rviz/nav2_default_view.rviz @@ -0,0 +1,671 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 462 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + camera_link: + Value: true + imu_link: + Value: true + laser_frame: + Value: true + map: + Value: true + odom: + Value: true + wheel_1: + Value: true + wheel_2: + Value: true + wheel_3: + Value: true + wheel_4: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + base_link: + camera_link: + {} + imu_link: + {} + laser_frame: + {} + wheel_1: + {} + wheel_2: + {} + wheel_3: + {} + wheel_4: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lookahead_collision_arc + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.20000000298023224 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lookahead_point + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5707999467849731 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 249.39932250976562 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0.8555871248245239 + Y: -4.082614421844482 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 932 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000020b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032010000024e000001390000012600fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1545 + X: 2193 + Y: 120 diff --git a/plc_bringup/rviz/nav2_namespaced_view.rviz b/plc_bringup/rviz/nav2_namespaced_view.rviz new file mode 100644 index 0000000..e95196a --- /dev/null +++ b/plc_bringup/rviz/nav2_namespaced_view.rviz @@ -0,0 +1,378 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 195 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1/Status1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1/Global Costmap1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 464 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.3 + Min Arrow Length: 0.02 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + Enabled: true + Name: Controller + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5708 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 160 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 914 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1545 + X: 186 + Y: 117 diff --git a/plc_bringup/urdf/myrobot b/plc_bringup/urdf/myrobot new file mode 100644 index 0000000..cd0674c --- /dev/null +++ b/plc_bringup/urdf/myrobot @@ -0,0 +1,483 @@ + + + + + 0.000456 0 0.000638 0 -0 0 + 8.23 + + 0.100968 + 0 + -0.000653858 + 0.101435 + 0 + 0.10047 + + + + 0 0 0 0 -0 0 + + + 0.2 0.3 0.1 + + + + + + + + + + + + + 0.125 0 0.175 0 -0 0 + + + 0.05 0.05 0.05 + + + + + + + + + + + + + 0.125 0 0.225 0 -0 0 + + + 0.05 0.05 0.05 + + + + + + + + + + + + + 0.125 0 0.125 0 -0 0 + + + 0.05 0.05 0.05 + + + + + 0 0 0.05 0 -0 1.5708 + + + 0.2 0.3 0.1 + + + + + + + + 0.125 0 0.175 0 -0 0 + + + 0.05 0.05 0.05 + + + + + + + + 0.125 0 0.225 0 -0 0 + + + 1 1 1 + /home/hshine/plan_control_ws/src/plc_bringup/meshes/hokuyo.dae + + + + + 0.125 0 0.125 0 -0 0 + + + 0.05 0.05 0.05 + + + + 1 + + 30 + + 1.39626 + + 1920 + 1080 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0 + 0.007 + + + + 1 + 0.0 + / + cam + camera_info + camera_link + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + 0.125 0 0.175 0 -0 0 + + + 1 + 0 + 15 + + + + 720 + 1 + -3.14159 + 3.14159 + + + + 0.05 + 30 + 0.01 + + + gaussian + 0 + 0.01 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + laser_frame + + 0.125 0 0.225 0 -0 0 + + + + 0.1 0.13 0 -1.5708 0 0 + base_footprint + wheel_1 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.5 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0 0 -0 0 + + + 0.05 + 0.06 + + + + + + + + + + + + + 0 0 0 0 -0 0 + + + 0.05 + 0.06 + + + + + + + + + -0.1 0.13 0 -1.5708 0 0 + base_footprint + wheel_2 + + 0 0 1 + + 100 + 100 + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.5 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0 0 -0 0 + + + 0.05 + 0.06 + + + + + + + + + + + + + 0 0 0 0 -0 0 + + + 0.05 + 0.06 + + + + + + + + + 0.1 -0.13 0 -1.5708 0 0 + base_footprint + wheel_3 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.5 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0 0 -0 0 + + + 0.05 + 0.06 + + + + + + + + + + + + + 0 0 0 0 -0 0 + + + 0.05 + 0.06 + + + + + + + + + -0.1 -0.13 0 -1.5708 0 0 + base_footprint + wheel_4 + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.5 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0 0 -0 0 + + + 0.05 + 0.06 + + + + + + + + + + + + + 0 0 0 0 -0 0 + + + 0.05 + 0.06 + + + + + + + + 0 + + + cmd_vel + odom + odom + base_to_wheel1 + base_to_wheel3 + base_to_wheel2 + base_to_wheel4 + 100 + base_footprint + 1 + + + + ~/out:=joint_states + + 30 + base_to_wheel1 + base_to_wheel2 + base_to_wheel3 + base_to_wheel4 + + + diff --git a/plc_bringup/urdf/myrobot.urdf b/plc_bringup/urdf/myrobot.urdf new file mode 100644 index 0000000..c40a2ee --- /dev/null +++ b/plc_bringup/urdf/myrobot.urdf @@ -0,0 +1,464 @@ + + + + + + + + + + + + Gazebo/Green + false + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Orange + + + Gazebo/Red + + + Gazebo/Black + + + Gazebo/Red + + + Gazebo/Black + + + + + + Gazebo/Red + + + + + + + true + true + 0 0 0 0 0 0 + 15 + + + + 720 + 1 + -3.1415926 + 3.1415926 + + + + 0.05 + 30.0 + 0.01 + + + gaussian + + 0.0 + 0.01 + + + + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + laser_frame + + + + + + + 30.0 + + 1.3962634 + + 1920 + 1080 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + true + 0.0 + / + cam + camera_info + camera_link + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + + + 2 + + + base_to_wheel1 + base_to_wheel3 + + + base_to_wheel2 + base_to_wheel4 + + + 0.26 + 0.26 + + 0.12 + 0.12 + + + 20 + 1.0 + + + true + true + true + + odom + base_footprint + + + + + + + + ~/out:=joint_states + + 30 + base_to_wheel1 + base_to_wheel2 + base_to_wheel3 + base_to_wheel4 + + + + + + + diff --git a/plc_bringup/urdf/turtlebot3_waffle.urdf b/plc_bringup/urdf/turtlebot3_waffle.urdf new file mode 100644 index 0000000..f56bd6d --- /dev/null +++ b/plc_bringup/urdf/turtlebot3_waffle.urdf @@ -0,0 +1,293 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/plc_bringup/urdf/waking_robot.urdf b/plc_bringup/urdf/waking_robot.urdf new file mode 100644 index 0000000..1ab6c55 --- /dev/null +++ b/plc_bringup/urdf/waking_robot.urdf @@ -0,0 +1,419 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + false + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Orange + + + Gazebo/Red + + + Gazebo/Black + + + Gazebo/Red + + + Gazebo/Black + + + + + + Gazebo/Red + + + + + + + true + true + 0 0 0 0 0 0 + + 15 + + + + 360 + 1 + -3.1415926 + 3.1415926 + + + + 0.05 + 30.0 + 0.01 + + + gaussian + + 0.0 + 0.01 + + + + + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + laser_frame + + + + + + + 30.0 + + 1.3962634 + + 1920 + 1080 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + true + 0.0 + / + cam + camera_info + camera_link + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + true + 100.0 + / + base_to_wheel1 + base_to_wheel3 + base_to_wheel2 + base_to_wheel4 + 0.26 + 0.12 + 2 + cmd_vel + odom + odom + dummy + 1 + + + + + Gazebo/Orange + true + + true + 100 + true + __default_topic__ + + false + + imu + imu_link + 100.0 + 0.0 + 0 0 0 + 0 0 0 + imu_link + + 0 0 0 0 0 0 + + + + diff --git a/plc_bringup/urdf/waking_robot.xacro b/plc_bringup/urdf/waking_robot.xacro new file mode 100644 index 0000000..f0d57bb --- /dev/null +++ b/plc_bringup/urdf/waking_robot.xacro @@ -0,0 +1,471 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + false + + + + + + + + + + + + + + + + 　　　 + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + 0.5 + 0.5 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Orange + + + + Gazebo/Red + + + + Gazebo/Black + + + + Gazebo/Red + + + + Gazebo/Black + + + + + + + + + + + Gazebo/Red + + + + + + + + true + true + 0 0 0 0 0 0 + 15 + + + + 720 + 1 + -3.1415926 + 3.1415926 + + + + 0.05 + 30.0 + 0.01 + + + gaussian + + 0.0 + 0.01 + + + + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + laser_frame + + + + + + + + 30.0 + + 1.3962634 + + 1920 + 1080 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + true + 0.0 + / + cam + camera_info + camera_link + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + true + 100.0 + / + base_to_wheel1 + base_to_wheel3 + base_to_wheel2 + base_to_wheel4 + 0.26 + 0.12 + 2 + cmd_vel + odom + odom + dummy + 1 + + + + + + + + + + diff --git a/plc_bringup/worlds/race_with_cone.world b/plc_bringup/worlds/race_with_cone.world new file mode 100644 index 0000000..d97f02a --- /dev/null +++ b/plc_bringup/worlds/race_with_cone.world @@ -0,0 +1,2062 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + +

+ + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + + 0 -3.68256 0 0 -0 0 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 7.25 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 7.25 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 3.55 -0.007561 0 0 0 -1.5708 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 7.5 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 7.5 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + -0.803754 2.63593 0 0 -0 0 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 5.5 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 5.5 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + -1.117 1.60767 0 0 -0 0 + 0 + 0 + 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0.27 0 0.25 0 -0 0 + + + 1.50561 0.15 0.5 + + + + + 1 1 1 1 + + __default__ + + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0.27 0 0.25 0 -0 0 + + + 1.50561 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + -0.029 0.557668 0 0 0 -1.5708 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 2.25 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 2.25 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + -1.079 -0.492332 0 0 -0 3.14159 + 0 + 0 + 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + -0.27 0 0.25 0 -0 0 + + + 1.6 0.15 0.5 + + + + + 1 1 1 1 + + __default__ + + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + -0.27 0 0.25 0 -0 0 + + + 1.6 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + -2.552 0.623668 0 0 -0 0 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 2 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 2 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 2.729 1.65167 0 0 -0 3.14159 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 1.5 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 1.5 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 0.977 0.647668 0 0 -0 3.14159 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 2.15 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 2.15 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 2.054 1.15167 0 0 0 -1.5708 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 1.15 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 1.15 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + -0.981 -1.57933 0 0 -0 0 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 5 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 5 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 1.444 -1.02933 0 0 -0 1.5708 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 1.25 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 1.25 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 1.619 -0.479332 0 0 -0 0 + 0 + 0 + 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0.15 0 0.25 0 -0 0 + + + 0.75 0.15 0.5 + + + + + 1 1 1 1 + + __default__ + + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0.15 0 0.25 0 -0 0 + + + 0.75 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + -3.55 -0.007561 0 0 -0 1.5708 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 7.5 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 7.5 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + 0 3.68256 0 0 -0 0 + 0 + 0 + 0 + + 0 0 0.25 0 -0 0 + + + 7.25 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + 0 + 1 + + + 0 + 10 + 0 0 0.25 0 -0 0 + + + 7.25 0.15 0.5 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + 1.35845 -3.20717 0 0 -0 0 + + + 3.795 -5.77379 0 0 -0 0 + + + + + 1.5 0.15 0.5 + + + 0 0 0.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 0.25 0 -0 0 + + + 1.5 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + + 1.51 -0.325 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 1 0.15 0.5 + + + 0 0 0.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 0.25 0 -0 0 + + + 1 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + + -1.51 0.575 0 0 -0 1.5708 + 0 + 0 + 0 + + 1 + + + 1 + + + + + 0 0 1 + 0.5 1.8 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 0.5 1.8 + + + + + + + 0 + 0 + 0 + + 0.084155 0.911194 0 0 -0 0 + + + 1 + + + + + 0 0 1 + 0.6 0.6 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 0.6 0.6 + + + + + + + 0 + 0 + 0 + + -0.089887 -5.43769 0 0 -0 0 + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 0 + 0 + + 5.72853 -4.73904 0 0 -0 0 + + + 106 134000000 + 109 680228648 + 1707129491 657314342 + 106134 + + 5.34999 -3.68726 -3e-06 1e-06 -2e-06 -0 + 1 1 1 + + 5.34999 -3.68726 -3e-06 1e-06 -2e-06 -0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 0.977897 -6.38807 -3e-06 1e-06 -1e-06 0 + 1 1 1 + + 0.977897 -6.38807 -3e-06 1e-06 -1e-06 0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 1.8477 -2.52559 -7e-06 -0 -0 -0 + 1 1 1 + + 1.8477 -2.52559 -7e-06 -0 -0 -0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 4.01007 -0.137731 -6e-06 0 -0 0 + 1 1 1 + + 4.01007 -0.137731 -6e-06 0 -0 0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 0.236515 -3.76484 -3e-06 1e-06 -1e-06 0 + 1 1 1 + + 0.236515 -3.76484 -3e-06 1e-06 -1e-06 0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 4.00495 -6.38786 -3e-06 1e-06 -1e-06 0 + 1 1 1 + + 4.00495 -6.38786 -3e-06 1e-06 -1e-06 0 + 0 0 0 0 -0 0 + 0 0 -9.8 -0 -0 -0 + 0 0 -9.8 0 -0 0 + + + + -0.202 -5.296 0 0 0 -1.57 + 1 1 1 + + -0.202 -5.296 0 0 0 -1.57 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 2.9706 -3.18013 0 0 -0 0 + 1 1 1 + + 2.9706 -6.86269 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 6.5206 -3.18769 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.16685 -0.5442 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.8536 -1.57246 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.9416 -2.62246 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.8916 -3.67246 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.4186 -2.55646 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.6996 -1.52846 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.9476 -2.53246 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 5.0246 -2.02846 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 1.9896 -4.75946 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.4146 -4.20946 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 4.5896 -3.65946 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.5794 -3.18769 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.9706 0.50243 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 3.99187 -5.79794 0 0 -0 0 + 1 1 1 + + 5.50187 -6.12294 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.48187 -5.22294 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.365307 -0.005 0 0 0 -1.56 + 1 1 1 + + 0.365307 -0.005 0 0 0 -1.56 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 0 + 0 + + 0.977897 -6.38807 0 0 -0 0 + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 0 + 0 + + 0.019899 -1.01047 0 0 -0 0 + + + + 2.24972 -5.19736 3.36028 0 0.73627 0.766073 + orbit + perspective + + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 0 + 0 + + 4.01007 -1.00114 0 0 -1e-06 0 + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 0 + 0 + + 0.236515 -3.76484 0 0 -1e-06 0 + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 5 5 5 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + 0 0 0 0 -0 0 + + 0 + 0 + + 4.00495 -6.38786 -3e-06 1e-06 -1e-06 0 + + + diff --git a/plc_bringup/worlds/waffle.model b/plc_bringup/worlds/waffle.model new file mode 100644 index 0000000..3c5923d --- /dev/null +++ b/plc_bringup/worlds/waffle.model @@ -0,0 +1,504 @@ + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_common/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + + + + + true + 200 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + false + + + ~/out:=imu + + + + + + + + -0.064 0 0.121 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.052 0 0.111 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.064 0 0.121 0 0 0 + + + model://turtlebot3_common/meshes/lds.dae + 0.001 0.001 0.001 + + + + + + true + true + -0.064 0 0.15 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 20.0 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_common/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_common/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + 0.069 -0.047 0.107 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 0 0 + + + 0.008 0.130 0.022 + + + + + 0.069 -0.047 0.107 0 0 0 + + + 1 + 5 + 0.064 -0.047 0.107 0 0 0 + + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + + 0 0.047 -0.005 0 0 0 + + + 0.008 0.130 0.022 + + + + 0.069 -0.047 0.107 0 0 0 + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 0 0 + + 0 0 1 + + + + + + + + + /tf:=tf + + + 30 + + + wheel_left_joint + wheel_right_joint + + + 0.287 + 0.066 + + + 20 + 1.0 + + cmd_vel + + + true + true + false + + odom + odom + base_footprint + + + + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + diff --git a/plc_bringup/worlds/waffle1.model b/plc_bringup/worlds/waffle1.model new file mode 100644 index 0000000..3c5923d --- /dev/null +++ b/plc_bringup/worlds/waffle1.model @@ -0,0 +1,504 @@ + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_common/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + + + + + true + 200 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + false + + + ~/out:=imu + + + + + + + + -0.064 0 0.121 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.052 0 0.111 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.064 0 0.121 0 0 0 + + + model://turtlebot3_common/meshes/lds.dae + 0.001 0.001 0.001 + + + + + + true + true + -0.064 0 0.15 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 20.0 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_common/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_common/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + + 0.069 -0.047 0.107 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.035 + + + 0 0.047 -0.005 0 0 0 + + + 0.008 0.130 0.022 + + + + + 0.069 -0.047 0.107 0 0 0 + + + 1 + 5 + 0.064 -0.047 0.107 0 0 0 + + + + + + intel_realsense_r200_depth + camera_depth_frame + 0.07 + 0.001 + 5.0 + + + + + 0 0.047 -0.005 0 0 0 + + + 0.008 0.130 0.022 + + + + 0.069 -0.047 0.107 0 0 0 + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + base_link + camera_link + 0.064 -0.065 0.094 0 0 0 + + 0 0 1 + + + + + + + + + /tf:=tf + + + 30 + + + wheel_left_joint + wheel_right_joint + + + 0.287 + 0.066 + + + 20 + 1.0 + + cmd_vel + + + true + true + false + + odom + odom + base_footprint + + + + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + diff --git a/plc_bringup/worlds/world_only.model b/plc_bringup/worlds/world_only.model new file mode 100644 index 0000000..aed412a --- /dev/null +++ b/plc_bringup/worlds/world_only.model @@ -0,0 +1,54 @@ + + + + + + model://ground_plane + + + + model://sun + + + + false + + + + + 0 0 10 0 1.570796 0 + orbit + perspective + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + 1 + + model://turtlebot3_world + + + + +