omni move gazebo simulate
This commit is contained in:
commit
7f4eb6f1bf
Binary file not shown.
|
|
@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 750 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 15 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 103 KiB |
|
|
@ -0,0 +1,7 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<model>
|
||||
<name>End Plane</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.4">model.sdf</sdf>
|
||||
<description>My textured ground plane.</description>
|
||||
</model>
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<sdf version="1.4">
|
||||
<model name="end_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
|
||||
<normal>0 0 1</normal>
|
||||
<size>0.6 0.6 0.0001</size>
|
||||
|
||||
</plane>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100</mu>
|
||||
<mu2>50</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>0.6 0.6 0.0001</size>
|
||||
</plane>
|
||||
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>model://end_plane/materials/scripts</uri>
|
||||
<uri>model://end_plane/materials/textures/</uri>
|
||||
<name>EndPlane/Image</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 750 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 15 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 103 KiB |
|
|
@ -0,0 +1,7 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<model>
|
||||
<name>Start Plane</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.4">model.sdf</sdf>
|
||||
<description>My textured ground plane.</description>
|
||||
</model>
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<sdf version="1.4">
|
||||
<model name="start_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
|
||||
<normal>0 0 1</normal>
|
||||
<size>0.5 1.8 0.0001</size>
|
||||
|
||||
</plane>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100</mu>
|
||||
<mu2>50</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>0.5 1.8 0.0001</size>
|
||||
</plane>
|
||||
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>model://start_plane/materials/scripts</uri>
|
||||
<uri>model://start_plane/materials/textures/</uri>
|
||||
<name>StartPlane/Image</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
|
|
@ -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()
|
||||
|
||||
|
|
@ -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.
|
||||
|
||||
<p align="center">
|
||||
<img src="https://user-images.githubusercontent.com/14944147/102563918-3cd49d80-408f-11eb-8e03-b472815a05d8.gif">
|
||||
</p>
|
||||
|
||||
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.
|
||||
|
||||
|
||||
|
||||
## 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.
|
||||
|
|
@ -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
|
||||
}
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 106 KiB |
|
|
@ -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 <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <algorithm>
|
||||
#include <mutex>
|
||||
|
||||
#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<tf2_ros::Buffer> tf,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> 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<geometry_msgs::msg::PointStamped> 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<rclcpp::Parameter> parameters);
|
||||
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
std::string plugin_name_;
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> 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<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
|
||||
carrot_pub_;
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
|
||||
std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
|
||||
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_
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
<class_libraries>
|
||||
<library path="omni_regulated_pure_pursuit_controller">
|
||||
<class type="omni_regulated_pure_pursuit_controller::RegulatedPurePursuitController" base_class_type="nav2_core::Controller">
|
||||
<description>
|
||||
omni_regulated_pure_pursuit_controller
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
</class_libraries>
|
||||
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>omni_regulated_pure_pursuit_controller</name>
|
||||
<version>1.1.13</version>
|
||||
<description>omni_Regulated Pure Pursuit Controller</description>
|
||||
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
|
||||
<maintainer email="shrijitsingh99@gmail.com">Shrijit Singh</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>nav2_common</depend>
|
||||
<depend>nav2_core</depend>
|
||||
<depend>nav2_util</depend>
|
||||
<depend>nav2_costmap_2d</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav2_msgs</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
<nav2_core plugin="${prefix}/omni_regulated_pure_pursuit_controller.xml" />
|
||||
</export>
|
||||
|
||||
</package>
|
||||
|
|
@ -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 <algorithm>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#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<tf2_ros::Buffer> tf,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> 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<nav_msgs::msg::Path>("received_global_plan", 1);
|
||||
carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>("lookahead_point", 1);
|
||||
carrot_arc_pub_ = node->create_publisher<nav_msgs::msg::Path>("lookahead_collision_arc", 1);
|
||||
|
||||
// initialize collision checker and set costmap
|
||||
collision_checker_ = std::make_unique<nav2_costmap_2d::
|
||||
FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(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<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController::createCarrotMsg(
|
||||
const geometry_msgs::msg::PoseStamped & carrot_pose)
|
||||
{
|
||||
auto carrot_msg = std::make_unique<geometry_msgs::msg::PointStamped>();
|
||||
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<std::mutex> lock_reinit(mutex_);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
|
||||
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> 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<double>(NO_INFORMATION) &&
|
||||
costmap_ros_->getLayeredCostmap()->isTrackingUnknown())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// if occupied or unknown and not to traverse unknown space
|
||||
return footprint_cost >= static_cast<double>(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<double>(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<double>(NO_INFORMATION) &&
|
||||
pose_cost != static_cast<double>(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<double>::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<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
std::lock_guard<std::mutex> 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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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 <memory>
|
||||
|
||||
#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<std::unique_ptr<PathSegment>> 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
|
||||
|
|
@ -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 <cmath>
|
||||
#include <initializer_list>
|
||||
#include <memory>
|
||||
|
||||
#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<std::unique_ptr<PathSegment>> segments);
|
||||
|
||||
} // namespace path_utils
|
||||
|
||||
#endif // PATH_UTILS__PATH_UTILS_HPP_
|
||||
|
|
@ -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 <memory>
|
||||
|
||||
#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<Straight>(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<RightTurnAround>(radius),
|
||||
});
|
||||
constexpr double expected_path_length = M_PI * radius;
|
||||
EXPECT_NEAR(path.poses.size(), 1 + static_cast<std::size_t>(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<Straight>(1.0),
|
||||
std::make_unique<LeftTurn>(1.0),
|
||||
std::make_unique<RightTurn>(1.0),
|
||||
std::make_unique<LeftTurnAround>(1.0),
|
||||
std::make_unique<RightTurnAround>(1.0),
|
||||
std::make_unique<LeftCircle>(1.0),
|
||||
std::make_unique<RightCircle>(1.0),
|
||||
std::make_unique<Arc>(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<std::size_t>(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);
|
||||
}
|
||||
|
|
@ -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 <math.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <limits>
|
||||
|
||||
#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<geometry_msgs::msg::PointStamped> 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<rclcpp_lifecycle::LifecycleNode>("testRPP");
|
||||
std::string name = "PathFollower";
|
||||
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
|
||||
|
||||
// instantiate
|
||||
auto ctrl = std::make_shared<BasicAPIRPP>();
|
||||
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<BasicAPIRPP>();
|
||||
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<BasicAPIRPP>();
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("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<double>::max());
|
||||
}
|
||||
|
||||
using CircleSegmentIntersectionParam = std::tuple<
|
||||
std::pair<double, double>,
|
||||
std::pair<double, double>,
|
||||
double,
|
||||
std::pair<double, double>
|
||||
>;
|
||||
|
||||
class CircleSegmentIntersectionTest
|
||||
: public ::testing::TestWithParam<CircleSegmentIntersectionParam>
|
||||
{};
|
||||
|
||||
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<double, double> 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<BasicAPIRPP>();
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testRPP");
|
||||
std::string name = "PathFollower";
|
||||
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("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<double>(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<BasicAPIRPP>();
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testRPP");
|
||||
std::string name = "PathFollower";
|
||||
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("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<BasicAPIRPP>();
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testRPP");
|
||||
std::string name = "PathFollower";
|
||||
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("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<path_utils::Straight>(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<path_utils::Straight>(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<rclcpp_lifecycle::LifecycleNode>("Smactest");
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
|
||||
costmap->on_configure(rclcpp_lifecycle::State());
|
||||
auto ctrl =
|
||||
std::make_unique<nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController>();
|
||||
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
|
||||
ctrl->configure(node, "test", tf, costmap);
|
||||
ctrl->activate();
|
||||
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
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<BasicAPIRPP>();
|
||||
node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testRPP");
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
|
||||
costmap_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("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<BasicAPIRPP> ctrl_;
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
|
||||
std::shared_ptr<tf2_ros::Buffer> 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<path_utils::LeftCircle>(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<path_utils::LeftCircle>(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<path_utils::LeftCircle>(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<path_utils::LeftCircle>(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_utils::Straight>(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_utils::Straight>(path_length),
|
||||
std::make_unique<path_utils::LeftTurnAround>(1.0),
|
||||
std::make_unique<path_utils::Straight>(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);
|
||||
}
|
||||
|
|
@ -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()
|
||||
|
|
@ -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 `<robot_name>_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 <remapping>/tf:=tf</remapping> <remapping>/tf_static:=tf_static</remapping> 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
|
||||
```
|
||||
|
|
@ -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.
|
||||
# '<robot_namespace>' 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 '<robot_namespace>' keyword for the replacements.
|
||||
params_file = ReplaceString(
|
||||
source_file=params_file,
|
||||
replacements={'<robot_namespace>': ('/', 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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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 '
|
||||
'<robot_namespace> 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={'<robot_namespace>': ('/', 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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -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
|
||||
Binary file not shown.
|
|
@ -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
|
||||
Binary file not shown.
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,33 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>plc_bringup</name>
|
||||
<version>1.1.13</version>
|
||||
<description>Bringup scripts and configurations for the Nav2 stack</description>
|
||||
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
|
||||
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
|
||||
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>nav2_common</build_depend>
|
||||
|
||||
<build_depend>navigation2</build_depend>
|
||||
<build_depend>launch_ros</build_depend>
|
||||
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>navigation2</exec_depend>
|
||||
<exec_depend>nav2_common</exec_depend>
|
||||
<exec_depend>slam_toolbox</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_cmake_pytest</test_depend>
|
||||
<test_depend>launch</test_depend>
|
||||
<test_depend>launch_testing</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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:
|
||||
# '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined.
|
||||
# It doesn't need to start with '/'
|
||||
topic: <robot_namespace>/scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
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:
|
||||
# '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined.
|
||||
# It doesn't need to start with '/'
|
||||
topic: <robot_namespace>/scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
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
|
||||
|
|
@ -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
|
||||
|
|
@ -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: <Fixed 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: <Fixed 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
|
||||
|
|
@ -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: <Fixed 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_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <robot_namespace>/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: <Fixed 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
|
||||
|
|
@ -0,0 +1,483 @@
|
|||
<sdf version='1.7'>
|
||||
<model name='robot'>
|
||||
<link name='base_footprint'>
|
||||
<inertial>
|
||||
<pose>0.000456 0 0.000638 0 -0 0</pose>
|
||||
<mass>8.23</mass>
|
||||
<inertia>
|
||||
<ixx>0.100968</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>-0.000653858</ixz>
|
||||
<iyy>0.101435</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.10047</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='base_footprint_fixed_joint_lump__base_link_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.2 0.3 0.1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<collision name='base_footprint_fixed_joint_lump__camera_link_collision_1'>
|
||||
<pose>0.125 0 0.175 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<collision name='base_footprint_fixed_joint_lump__laser_frame_collision_2'>
|
||||
<pose>0.125 0 0.225 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<collision name='base_footprint_fixed_joint_lump__imu_link_collision_3'>
|
||||
<pose>0.125 0 0.125 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='base_footprint_fixed_joint_lump__base_link_visual'>
|
||||
<pose>0 0 0.05 0 -0 1.5708</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.2 0.3 0.1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Orange</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name='base_footprint_fixed_joint_lump__camera_link_visual_1'>
|
||||
<pose>0.125 0 0.175 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Red</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name='base_footprint_fixed_joint_lump__laser_frame_visual_2'>
|
||||
<pose>0.125 0 0.225 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>/home/hshine/plan_control_ws/src/plc_bringup/meshes/hokuyo.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='base_footprint_fixed_joint_lump__imu_link_visual_3'>
|
||||
<pose>0.125 0 0.125 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<sensor name='camera1' type='camera'>
|
||||
<update_rate>30</update_rate>
|
||||
<camera name='head'>
|
||||
<horizontal_fov>1.39626</horizontal_fov>
|
||||
<image>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<plugin name='camera_controller' filename='libgazebo_ros_camera.so'>
|
||||
<alwaysOn>1</alwaysOn>
|
||||
<updateRate>0.0</updateRate>
|
||||
<cameraName>/</cameraName>
|
||||
<imageTopicName>cam</imageTopicName>
|
||||
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
|
||||
<frameName>camera_link</frameName>
|
||||
<hackBaseline>0.07</hackBaseline>
|
||||
<distortionK1>0.0</distortionK1>
|
||||
<distortionK2>0.0</distortionK2>
|
||||
<distortionK3>0.0</distortionK3>
|
||||
<distortionT1>0.0</distortionT1>
|
||||
<distortionT2>0.0</distortionT2>
|
||||
</plugin>
|
||||
<pose>0.125 0 0.175 0 -0 0</pose>
|
||||
</sensor>
|
||||
<sensor name='head_hokuyo_sensor' type='ray'>
|
||||
<always_on>1</always_on>
|
||||
<visualize>0</visualize>
|
||||
<update_rate>15</update_rate>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>720</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>-3.14159</min_angle>
|
||||
<max_angle>3.14159</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.05</min>
|
||||
<max>30</max>
|
||||
<resolution>0.01</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<plugin name='turtlebot3_laserscan' filename='libgazebo_ros_ray_sensor.so'>
|
||||
<ros>
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frameName>laser_frame</frameName>
|
||||
</plugin>
|
||||
<pose>0.125 0 0.225 0 -0 0</pose>
|
||||
</sensor>
|
||||
</link>
|
||||
<joint name='base_to_wheel1' type='revolute'>
|
||||
<pose relative_to='base_footprint'>0.1 0.13 0 -1.5708 0 0</pose>
|
||||
<parent>base_footprint</parent>
|
||||
<child>wheel_1</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='wheel_1'>
|
||||
<pose relative_to='base_to_wheel1'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<mass>0.5</mass>
|
||||
<inertia>
|
||||
<ixx>0.1</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.1</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.1</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='wheel_1_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.05</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='wheel_1_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.05</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Red</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='base_to_wheel2' type='revolute'>
|
||||
<pose relative_to='base_footprint'>-0.1 0.13 0 -1.5708 0 0</pose>
|
||||
<parent>base_footprint</parent>
|
||||
<child>wheel_2</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<effort>100</effort>
|
||||
<velocity>100</velocity>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='wheel_2'>
|
||||
<pose relative_to='base_to_wheel2'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<mass>0.5</mass>
|
||||
<inertia>
|
||||
<ixx>0.1</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.1</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.1</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='wheel_2_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.05</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='wheel_2_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.05</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Black</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='base_to_wheel3' type='revolute'>
|
||||
<pose relative_to='base_footprint'>0.1 -0.13 0 -1.5708 0 0</pose>
|
||||
<parent>base_footprint</parent>
|
||||
<child>wheel_3</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='wheel_3'>
|
||||
<pose relative_to='base_to_wheel3'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<mass>0.5</mass>
|
||||
<inertia>
|
||||
<ixx>0.1</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.1</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.1</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='wheel_3_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.05</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='wheel_3_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.05</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Red</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='base_to_wheel4' type='revolute'>
|
||||
<pose relative_to='base_footprint'>-0.1 -0.13 0 -1.5708 0 0</pose>
|
||||
<parent>base_footprint</parent>
|
||||
<child>wheel_4</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='wheel_4'>
|
||||
<pose relative_to='base_to_wheel4'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<mass>0.5</mass>
|
||||
<inertia>
|
||||
<ixx>0.1</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.1</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.1</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='wheel_4_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.05</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='wheel_4_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.05</length>
|
||||
<radius>0.06</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Black</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<static>0</static>
|
||||
<!-- <plugin name='skid_steer_drive' filename='libgazebo_ros_diff_drive.so'>
|
||||
<num_wheel_pairs>2</num_wheel_pairs>
|
||||
<left_joint>base_to_wheel1</left_joint>
|
||||
<right_joint>base_to_wheel3</right_joint>
|
||||
<left_joint>base_to_wheel2</left_joint>
|
||||
<right_joint>base_to_wheel4</right_joint>
|
||||
<wheel_separation>0.26</wheel_separation>
|
||||
<wheel_separation>0.26</wheel_separation>
|
||||
<wheel_diameter>0.12</wheel_diameter>
|
||||
<wheel_diameter>0.12</wheel_diameter>
|
||||
<max_wheel_torque>20</max_wheel_torque>
|
||||
<max_wheel_acceleration>1.0</max_wheel_acceleration>
|
||||
<publish_odom>1</publish_odom>
|
||||
<publish_odom_tf>1</publish_odom_tf>
|
||||
<publish_wheel_tf>1</publish_wheel_tf>
|
||||
<odometry_frame>odom</odometry_frame>
|
||||
<robot_base_frame>base_footprint</robot_base_frame>
|
||||
</plugin> -->
|
||||
<plugin name="mecanum_controller" filename="libgazebo_ros_planar_move.so">
|
||||
<commandTopic>cmd_vel</commandTopic>
|
||||
<odometryTopic>odom</odometryTopic>
|
||||
<odometryFrame>odom</odometryFrame>
|
||||
<leftFrontJoint>base_to_wheel1</leftFrontJoint>
|
||||
<rightFrontJoint>base_to_wheel3</rightFrontJoint>
|
||||
<leftRearJoint>base_to_wheel2</leftRearJoint>
|
||||
<rightRearJoint>base_to_wheel4</rightRearJoint>
|
||||
<odometryRate>100</odometryRate>
|
||||
<robotBaseFrame>base_footprint</robotBaseFrame>
|
||||
<broadcastTF>1</broadcastTF>
|
||||
</plugin>
|
||||
<plugin name='turtlebot3_joint_state' filename='libgazebo_ros_joint_state_publisher.so'>
|
||||
<ros>
|
||||
<remapping>~/out:=joint_states</remapping>
|
||||
</ros>
|
||||
<update_rate>30</update_rate>
|
||||
<joint_name>base_to_wheel1</joint_name>
|
||||
<joint_name>base_to_wheel2</joint_name>
|
||||
<joint_name>base_to_wheel3</joint_name>
|
||||
<joint_name>base_to_wheel4</joint_name>
|
||||
</plugin>
|
||||
</model>
|
||||
</sdf>
|
||||
|
|
@ -0,0 +1,464 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from waking_robot.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="robot">
|
||||
<link name="base_footprint">
|
||||
</link>
|
||||
<!-- <link name="base_footprint">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="0.0001"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="dummy_joint" type="fixed">
|
||||
<parent link="dummy"/>
|
||||
<child link="base_footprint"/>
|
||||
</joint> -->
|
||||
<!-- <xacro:include filename="$(find nav_sim)/urdf/robot.gazebo" /> -->
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Green</material>
|
||||
<turnGravityOff>false</turnGravityOff>
|
||||
</gazebo>
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<origin xyz="0 0 0"/>
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.2 .3 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 1.5707963" xyz="0 0 0.05"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.2 .3 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="8.2"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wheel_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="0.1 0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wheel_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="-0.1 0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wheel_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="0.1 -0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wheel_4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="-0.1 -0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_wheel1" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_1"/>
|
||||
<origin rpy="-1.5707963 0 0" xyz="0.1 0.13 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<joint name="base_to_wheel2" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<anchor xyz="0 0 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_2"/>
|
||||
<origin rpy="-1.5707963 0 0" xyz="-0.1 0.13 0"/>
|
||||
</joint>
|
||||
<joint name="base_to_wheel3" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<child link="wheel_3"/>
|
||||
<origin rpy="-1.5707963 0 0" xyz="0.1 -0.13 0"/>
|
||||
</joint>
|
||||
<joint name="base_to_wheel4" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<child link="wheel_4"/>
|
||||
<origin rpy="-1.5707963 0 0" xyz="-0.1 -0.13 0"/>
|
||||
</joint>
|
||||
<!-- IMU joint -->
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<axis xyz="0 1 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.125 0 0.125"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="imu_link"/>
|
||||
</joint>
|
||||
<!-- IMU -->
|
||||
<link name="imu_link">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="green">
|
||||
<color rgba="0 1 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="1e-2"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- IMU -->
|
||||
<!-- Camera joint -->
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<axis xyz="0 1 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.125 0 0.175"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="camera_link"/>
|
||||
</joint>
|
||||
<!-- Camera -->
|
||||
<link name="camera_link">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="red">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="1e-2"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- camera -->
|
||||
<!-- Hokuyo joint -->
|
||||
<joint name="hokuyo_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.125 0.0 0.225"/>
|
||||
<parent link="base_link"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<child link="laser_frame"/>
|
||||
</joint>
|
||||
<!-- Hokuyo Laser -->
|
||||
<link name="laser_frame">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/hshine/plan_control_ws/src/plc_bringup/meshes/hokuyo.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="1e-2"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- hokuyo -->
|
||||
<!-- gazebo插件设置相关 -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Orange</material>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_1">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_2">
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_3">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_4">
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
<!-- ros_control plugin -->
|
||||
<!-- <gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/robot</robotNamespace>
|
||||
<legacyModeNS> true </legacyModeNS>
|
||||
</plugin>
|
||||
</gazebo> -->
|
||||
<!-- camera_link -->
|
||||
<gazebo reference="camera_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
<!-- hokuyo -->
|
||||
<gazebo reference="laser_frame">
|
||||
<sensor name="head_hokuyo_sensor" type="ray">
|
||||
<!-- <sensor type="gpu_ray" name="head_hokuyo_sensor"> -->
|
||||
<!-- GPU -->
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<update_rate>15</update_rate>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>720</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>-3.1415926</min_angle>
|
||||
<max_angle>3.1415926</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.05</min>
|
||||
<max>30.0</max>
|
||||
<resolution>0.01</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise parameters based on published spec for Hokuyo laser
|
||||
achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
|
||||
stddev of 0.01m will put 99.7% of samples within 0.03m of the true
|
||||
reading. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<!-- GPU版本 -->
|
||||
<!-- <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so"> -->
|
||||
<!-- 换成TURTLEBOT的激光雷达 -->
|
||||
<plugin filename="libgazebo_ros_ray_sensor.so" name="turtlebot3_laserscan">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frameName>laser_frame</frameName>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
<!-- camera -->
|
||||
<gazebo reference="camera_link">
|
||||
<sensor name="camera1" type="camera">
|
||||
<update_rate>30.0</update_rate>
|
||||
<camera name="head">
|
||||
<horizontal_fov>1.3962634</horizontal_fov>
|
||||
<image>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise is sampled independently per pixel on each frame.
|
||||
That pixel's noise value is added to each of its color
|
||||
channels, which at that point lie in the range [0,1]. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<plugin filename="libgazebo_ros_camera.so" name="camera_controller">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>0.0</updateRate>
|
||||
<cameraName>/</cameraName>
|
||||
<imageTopicName>cam</imageTopicName>
|
||||
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
|
||||
<frameName>camera_link</frameName>
|
||||
<hackBaseline>0.07</hackBaseline>
|
||||
<distortionK1>0.0</distortionK1>
|
||||
<distortionK2>0.0</distortionK2>
|
||||
<distortionK3>0.0</distortionK3>
|
||||
<distortionT1>0.0</distortionT1>
|
||||
<distortionT2>0.0</distortionT2>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
<!-- Drive controller -->
|
||||
<gazebo>
|
||||
<!-- <plugin filename="libgazebo_ros_skid_steer_drive.so" name="skid_steer_drive_controller">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>100.0</updateRate>
|
||||
<robotNamespace>/</robotNamespace>
|
||||
<leftFrontJoint>base_to_wheel1</leftFrontJoint>
|
||||
<rightFrontJoint>base_to_wheel3</rightFrontJoint>
|
||||
<leftRearJoint>base_to_wheel2</leftRearJoint>
|
||||
<rightRearJoint>base_to_wheel4</rightRearJoint>
|
||||
<wheelSeparation>0.26</wheelSeparation>
|
||||
<wheelDiameter>0.12</wheelDiameter>
|
||||
<torque>2</torque>
|
||||
<commandTopic>cmd_vel</commandTopic>
|
||||
<odometryTopic>odom</odometryTopic>
|
||||
<odometryFrame>odom</odometryFrame>
|
||||
<robotBaseFrame>dummy</robotBaseFrame>
|
||||
<broadcastTF>1</broadcastTF>
|
||||
</plugin> -->
|
||||
<plugin name='skid_steer_drive' filename='libgazebo_ros_diff_drive.so'>
|
||||
|
||||
<!-- <ros>
|
||||
<remapping>cmd_vel:=cmd_demo</remapping>
|
||||
<remapping>odom:=odom_demo</remapping>
|
||||
</ros> -->
|
||||
|
||||
<!-- Number of wheel pairs -->
|
||||
<num_wheel_pairs>2</num_wheel_pairs>
|
||||
|
||||
<!-- wheels0 -->
|
||||
<left_joint>base_to_wheel1</left_joint>
|
||||
<right_joint>base_to_wheel3</right_joint>
|
||||
|
||||
<!-- wheels1-->
|
||||
<left_joint>base_to_wheel2</left_joint>
|
||||
<right_joint>base_to_wheel4</right_joint>
|
||||
|
||||
<!-- kinematics -->
|
||||
<wheel_separation>0.26</wheel_separation>
|
||||
<wheel_separation>0.26</wheel_separation>
|
||||
|
||||
<wheel_diameter>0.12</wheel_diameter>
|
||||
<wheel_diameter>0.12</wheel_diameter>
|
||||
|
||||
<!-- limits -->
|
||||
<max_wheel_torque>20</max_wheel_torque>
|
||||
<max_wheel_acceleration>1.0</max_wheel_acceleration>
|
||||
|
||||
<!-- output -->
|
||||
<publish_odom>true</publish_odom>
|
||||
<publish_odom_tf>true</publish_odom_tf>
|
||||
<publish_wheel_tf>true</publish_wheel_tf>
|
||||
|
||||
<odometry_frame>odom</odometry_frame>
|
||||
<robot_base_frame>base_footprint</robot_base_frame>
|
||||
|
||||
</plugin>
|
||||
|
||||
|
||||
<plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=joint_states</remapping>
|
||||
</ros>
|
||||
<update_rate>30</update_rate>
|
||||
<joint_name>base_to_wheel1</joint_name>
|
||||
<joint_name>base_to_wheel2</joint_name>
|
||||
<joint_name>base_to_wheel3</joint_name>
|
||||
<joint_name>base_to_wheel4</joint_name>
|
||||
</plugin>
|
||||
|
||||
</gazebo>
|
||||
<!-- imu控制 -->
|
||||
<!-- <gazebo reference="imu_link">
|
||||
<material>Gazebo/Orange</material>
|
||||
<gravity>true</gravity>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>100</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<topic>__default_topic__</topic>
|
||||
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
|
||||
<topicName>imu</topicName>
|
||||
<bodyName>imu_link</bodyName>
|
||||
<updateRateHZ>100.0</updateRateHZ>
|
||||
<gaussianNoise>0.0</gaussianNoise>
|
||||
<xyzOffset>0 0 0</xyzOffset>
|
||||
<rpyOffset>0 0 0</rpyOffset>
|
||||
<frameName>imu_link</frameName>
|
||||
</plugin>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
</sensor>
|
||||
</gazebo> -->
|
||||
<!-- gazebo插件设置相关结束 -->
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,293 @@
|
|||
<?xml version="1.0" ?>
|
||||
<robot name="turtlebot3_waffle" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<!-- <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.urdf"/>
|
||||
|
||||
<xacro:property name="r200_cam_rgb_px" value="0.005"/>
|
||||
<xacro:property name="r200_cam_rgb_py" value="0.018"/>
|
||||
<xacro:property name="r200_cam_rgb_pz" value="0.013"/>
|
||||
<xacro:property name="r200_cam_depth_offset" value="0.01"/> -->
|
||||
|
||||
<!-- Init colour -->
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="dark">
|
||||
<color rgba="0.3 0.3 0.3 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="light_black">
|
||||
<color rgba="0.4 0.4 0.4 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="green">
|
||||
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="grey">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.4235 0.0392 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="brown">
|
||||
<color rgba="0.8706 0.8118 0.7647 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="red">
|
||||
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="white">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link" />
|
||||
<origin xyz="0 0 0.010" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin xyz="-0.064 0 0.0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/waffle_base.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="light_black"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="-0.064 0 0.047" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.266 0.266 0.094"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="1.3729096e+00"/>
|
||||
<inertia ixx="8.7002718e-03" ixy="-4.7576583e-05" ixz="1.1160499e-04"
|
||||
iyy="8.6195418e-03" iyz="-3.5422299e-06"
|
||||
izz="1.4612727e-02" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="wheel_left_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_left_link"/>
|
||||
<origin xyz="0.0 0.144 0.023" rpy="-1.57 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<link name="wheel_left_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="1.57 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/tire.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="dark"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.018" radius="0.033"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="2.8498940e-02" />
|
||||
<inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
|
||||
iyy="1.1192413e-05" iyz="-1.4400107e-11"
|
||||
izz="2.0712558e-05" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="wheel_right_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_right_link"/>
|
||||
<origin xyz="0.0 -0.144 0.023" rpy="-1.57 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<link name="wheel_right_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="1.57 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/tire.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="dark"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.018" radius="0.033"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="2.8498940e-02" />
|
||||
<inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
|
||||
iyy="1.1192413e-05" iyz="-1.4400107e-11"
|
||||
izz="2.0712558e-05" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="caster_back_right_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="caster_back_right_link"/>
|
||||
<origin xyz="-0.177 -0.064 -0.004" rpy="-1.57 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="caster_back_right_link">
|
||||
<collision>
|
||||
<origin xyz="0 0.001 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.030 0.009 0.020"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.005" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.001" iyz="0.0"
|
||||
izz="0.001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="caster_back_left_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="caster_back_left_link"/>
|
||||
<origin xyz="-0.177 0.064 -0.004" rpy="-1.57 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="caster_back_left_link">
|
||||
<collision>
|
||||
<origin xyz="0 0.001 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.030 0.009 0.020"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.005" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.001" iyz="0.0"
|
||||
izz="0.001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="imu_link"/>
|
||||
<origin xyz="0.0 0 0.068" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="imu_link"/>
|
||||
|
||||
<joint name="scan_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="base_scan"/>
|
||||
<origin xyz="-0.064 0 0.122" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="base_scan">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/lds.dae" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="dark"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0.015 0 -0.0065" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0315" radius="0.055"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<mass value="0.114" />
|
||||
<origin xyz="0 0 0" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.001" iyz="0.0"
|
||||
izz="0.001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<origin xyz="0.064 -0.065 0.094" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="camera_link"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="1.57 0 1.57"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/r200.dae" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0.003 0.065 0.007" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.012 0.132 0.020"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<!-- This inertial field needs doesn't contain reliable data!! -->
|
||||
<!-- <inertial>
|
||||
<mass value="0.564" />
|
||||
<origin xyz="0 0 0" />
|
||||
<inertia ixx="0.003881243" ixy="0.0" ixz="0.0"
|
||||
iyy="0.000498940" iyz="0.0"
|
||||
izz="0.003879257" />
|
||||
</inertial>-->
|
||||
</link>
|
||||
|
||||
<joint name="camera_rgb_joint" type="fixed">
|
||||
<origin xyz="0.005 0.018 0.013" rpy="0 0 0"/>
|
||||
<!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py} ${r200_cam_rgb_pz}" rpy="0 0 0"/> -->
|
||||
<parent link="camera_link"/>
|
||||
<child link="camera_rgb_frame"/>
|
||||
</joint>
|
||||
<link name="camera_rgb_frame"/>
|
||||
|
||||
<joint name="camera_rgb_optical_joint" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
|
||||
<parent link="camera_rgb_frame"/>
|
||||
<child link="camera_rgb_optical_frame"/>
|
||||
</joint>
|
||||
<link name="camera_rgb_optical_frame"/>
|
||||
|
||||
<joint name="camera_depth_joint" type="fixed">
|
||||
<origin xyz="0.005 0.028 0.013" rpy="-1.57 0 -1.57"/>
|
||||
<!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py + r200_cam_depth_offset} ${r200_cam_rgb_pz}" rpy="0 0 0"/> -->
|
||||
<parent link="camera_link"/>
|
||||
<child link="camera_depth_frame"/>
|
||||
</joint>
|
||||
<link name="camera_depth_frame"/>
|
||||
|
||||
<joint name="camera_depth_optical_joint" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="camera_depth_frame"/>
|
||||
<child link="camera_depth_optical_frame"/>
|
||||
</joint>
|
||||
<link name="camera_depth_optical_frame"/>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,419 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from waking_robot.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="robot">
|
||||
<link name="dummy">
|
||||
</link>
|
||||
<link name="base_footprint">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="0.0001"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="dummy_joint" type="fixed">
|
||||
<parent link="dummy"/>
|
||||
<child link="base_footprint"/>
|
||||
</joint>
|
||||
<!-- <xacro:include filename="$(find nav_sim)/urdf/robot.gazebo" /> -->
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Green</material>
|
||||
<turnGravityOff>false</turnGravityOff>
|
||||
</gazebo>
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<origin xyz="0 0 0"/>
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link"/>
|
||||
</joint>
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.2 .3 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 1.5707963" xyz="0 0 0.05"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.2 .3 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="8.2"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wheel_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="0.1 0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wheel_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="-0.1 0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wheel_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="0.1 -0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="wheel_4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="-0.1 -0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.05" radius="0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<inertial>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_wheel1" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_1"/>
|
||||
<origin rpy="-1.5707963 0 0" xyz="0.1 0.13 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
<joint name="base_to_wheel2" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<anchor xyz="0 0 0"/>
|
||||
<limit effort="100" velocity="100"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_2"/>
|
||||
<origin rpy="-1.5707963 0 0" xyz="-0.1 0.13 0"/>
|
||||
</joint>
|
||||
<joint name="base_to_wheel3" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<child link="wheel_3"/>
|
||||
<origin rpy="-1.5707963 0 0" xyz="0.1 -0.13 0"/>
|
||||
</joint>
|
||||
<joint name="base_to_wheel4" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<child link="wheel_4"/>
|
||||
<origin rpy="-1.5707963 0 0" xyz="-0.1 -0.13 0"/>
|
||||
</joint>
|
||||
<!-- IMU joint -->
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<axis xyz="0 1 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.125 0 0.125"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="imu_link"/>
|
||||
</joint>
|
||||
<!-- IMU -->
|
||||
<link name="imu_link">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="green">
|
||||
<color rgba="0 1 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="1e-2"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- IMU -->
|
||||
<!-- Camera joint -->
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<axis xyz="0 1 0"/>
|
||||
<origin rpy="0 0 0" xyz="0.125 0 0.175"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="camera_link"/>
|
||||
</joint>
|
||||
<!-- Camera -->
|
||||
<link name="camera_link">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="red">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="1e-2"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- camera -->
|
||||
<!-- Hokuyo joint -->
|
||||
<joint name="hokuyo_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.125 0.0 0.225"/>
|
||||
<parent link="base_link"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<child link="laser_frame"/>
|
||||
</joint>
|
||||
<!-- Hokuyo Laser -->
|
||||
<link name="laser_frame">
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://plc_bringup/meshes/hokuyo.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="1e-2"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- hokuyo -->
|
||||
<!-- gazebo插件设置相关 -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Orange</material>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_1">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_2">
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_3">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
<gazebo reference="wheel_4">
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
<!-- ros_control plugin -->
|
||||
<!-- <gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/robot</robotNamespace>
|
||||
<legacyModeNS> true </legacyModeNS>
|
||||
</plugin>
|
||||
</gazebo> -->
|
||||
<!-- camera_link -->
|
||||
<gazebo reference="camera_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
<!-- hokuyo -->
|
||||
<gazebo reference="laser_frame">
|
||||
<sensor name="head_hokuyo_sensor" type="ray">
|
||||
<!-- <sensor type="gpu_ray" name="head_hokuyo_sensor"> -->
|
||||
<!-- GPU -->
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
|
||||
<update_rate>15</update_rate>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>360</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>-3.1415926</min_angle>
|
||||
<max_angle>3.1415926</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.05</min>
|
||||
<max>30.0</max>
|
||||
<resolution>0.01</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise parameters based on published spec for Hokuyo laser
|
||||
achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
|
||||
stddev of 0.01m will put 99.7% of samples within 0.03m of the true
|
||||
reading. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<!-- GPU版本 -->
|
||||
<!-- <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so"> -->
|
||||
<!-- 换一个turtlebot同款 -->
|
||||
<!-- <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_head_hokuyo_controller">
|
||||
<topicName>/scan</topicName>
|
||||
<frameName>laser_frame</frameName>
|
||||
</plugin> -->
|
||||
<plugin name="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frame_name>laser_frame</frame_name>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
<!-- camera -->
|
||||
<gazebo reference="camera_link">
|
||||
<sensor name="camera1" type="camera">
|
||||
<update_rate>30.0</update_rate>
|
||||
<camera name="head">
|
||||
<horizontal_fov>1.3962634</horizontal_fov>
|
||||
<image>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise is sampled independently per pixel on each frame.
|
||||
That pixel's noise value is added to each of its color
|
||||
channels, which at that point lie in the range [0,1]. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<plugin filename="libgazebo_ros_camera.so" name="camera_controller">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>0.0</updateRate>
|
||||
<cameraName>/</cameraName>
|
||||
<imageTopicName>cam</imageTopicName>
|
||||
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
|
||||
<frameName>camera_link</frameName>
|
||||
<hackBaseline>0.07</hackBaseline>
|
||||
<distortionK1>0.0</distortionK1>
|
||||
<distortionK2>0.0</distortionK2>
|
||||
<distortionK3>0.0</distortionK3>
|
||||
<distortionT1>0.0</distortionT1>
|
||||
<distortionT2>0.0</distortionT2>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
<!-- Drive controller -->
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_skid_steer_drive.so" name="skid_steer_drive_controller">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>100.0</updateRate>
|
||||
<robotNamespace>/</robotNamespace>
|
||||
<leftFrontJoint>base_to_wheel1</leftFrontJoint>
|
||||
<rightFrontJoint>base_to_wheel3</rightFrontJoint>
|
||||
<leftRearJoint>base_to_wheel2</leftRearJoint>
|
||||
<rightRearJoint>base_to_wheel4</rightRearJoint>
|
||||
<wheelSeparation>0.26</wheelSeparation>
|
||||
<wheelDiameter>0.12</wheelDiameter>
|
||||
<torque>2</torque>
|
||||
<commandTopic>cmd_vel</commandTopic>
|
||||
<odometryTopic>odom</odometryTopic>
|
||||
<odometryFrame>odom</odometryFrame>
|
||||
<robotBaseFrame>dummy</robotBaseFrame>
|
||||
<broadcastTF>1</broadcastTF>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<!-- imu控制 -->
|
||||
<gazebo reference="imu_link">
|
||||
<material>Gazebo/Orange</material>
|
||||
<gravity>true</gravity>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>100</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<topic>__default_topic__</topic>
|
||||
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
|
||||
<initial_orientation_as_reference>false</initial_orientation_as_reference>
|
||||
|
||||
<topicName>imu</topicName>
|
||||
<bodyName>imu_link</bodyName>
|
||||
<updateRateHZ>100.0</updateRateHZ>
|
||||
<gaussianNoise>0.0</gaussianNoise>
|
||||
<xyzOffset>0 0 0</xyzOffset>
|
||||
<rpyOffset>0 0 0</rpyOffset>
|
||||
<frameName>imu_link</frameName>
|
||||
</plugin>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
<!-- gazebo插件设置相关结束 -->
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,471 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
|
||||
<xacro:property name="length_wheel" value="0.05" />
|
||||
<xacro:property name="radius_wheel" value="0.06" />
|
||||
<xacro:property name="camera_link" value="0.05" />
|
||||
<xacro:property name="PI" value="3.1415926"/>
|
||||
<xacro:macro name="default_inertial" params="mass">
|
||||
<inertial>
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="0.1" ixy="0.0"
|
||||
ixz="0.0"
|
||||
iyy="0.1" iyz="0.0"
|
||||
izz="0.1" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
<link name="dummy">
|
||||
</link>
|
||||
<link name="base_footprint">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</visual>
|
||||
<xacro:default_inertial mass="0.0001"/>
|
||||
</link>
|
||||
<joint name="dummy_joint" type="fixed">
|
||||
<parent link="dummy"/>
|
||||
<child link="base_footprint"/>
|
||||
</joint>
|
||||
<!-- <xacro:include filename="$(find nav_sim)/urdf/robot.gazebo" /> -->
|
||||
|
||||
<gazebo reference="base_footprint">
|
||||
<material>Gazebo/Green</material>
|
||||
<turnGravityOff>false</turnGravityOff>
|
||||
</gazebo>
|
||||
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<origin xyz="0 0 0" />
|
||||
<parent link="base_footprint" />
|
||||
<child link="base_link" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.2 .3 .1"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 ${PI/2}" xyz="0 0 0.05"/>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.2 .3 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:default_inertial mass="8.2"/>
|
||||
</link>
|
||||
|
||||
<link name="wheel_1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="0.1 0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<!-- <mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2> -->
|
||||
<xacro:default_inertial mass="0.5"/>
|
||||
</link>
|
||||
|
||||
|
||||
|
||||
<link name="wheel_2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="-0.1 0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<!-- <mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2> -->
|
||||
<xacro:default_inertial mass="0.5"/>
|
||||
|
||||
</link>
|
||||
|
||||
<link name="wheel_3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="0.1 -0.1 0"/> -->
|
||||
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<!-- <mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2> -->
|
||||
<xacro:default_inertial mass="0.5"/>
|
||||
</link>
|
||||
|
||||
<link name="wheel_4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
|
||||
</geometry>
|
||||
<!-- <origin rpy="0 1.5 0" xyz="-0.1 -0.1 0"/> -->
|
||||
<origin rpy="0 0 0" xyz="0 0 0" />
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<!-- <mu1>0.5</mu1>
|
||||
<mu2>0.5</mu2> -->
|
||||
<xacro:default_inertial mass="0.5"/>
|
||||
|
||||
</link>
|
||||
|
||||
|
||||
|
||||
<joint name="base_to_wheel1" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_1"/>
|
||||
<origin rpy="${-PI/2} 0 0" xyz="0.1 0.13 0"/>
|
||||
<axis xyz="0 0 1" />
|
||||
</joint>
|
||||
|
||||
<joint name="base_to_wheel2" type="continuous">
|
||||
<axis xyz="0 0 1" />
|
||||
<anchor xyz="0 0 0" />
|
||||
<limit effort="100" velocity="100" />
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_2"/>
|
||||
<origin rpy="${-PI/2} 0 0" xyz="-0.1 0.13 0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_to_wheel3" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<axis xyz="0 0 1" />
|
||||
<child link="wheel_3"/>
|
||||
<origin rpy="${-PI/2} 0 0" xyz="0.1 -0.13 0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_to_wheel4" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<axis xyz="0 0 1" />
|
||||
<child link="wheel_4"/>
|
||||
<origin rpy="${-PI/2} 0 0" xyz="-0.1 -0.13 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- IMU joint -->
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<axis xyz="0 1 0" />
|
||||
<origin xyz="0.125 0 0.125" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="imu_link"/>
|
||||
</joint>
|
||||
|
||||
<!-- IMU -->
|
||||
<link name="imu_link">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="green">
|
||||
<color rgba="0 1 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<inertial>
|
||||
<mass value="1e-2" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<!-- IMU -->
|
||||
|
||||
<!-- Camera joint -->
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<axis xyz="0 1 0" />
|
||||
<origin xyz="0.125 0 0.175" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="camera_link"/>
|
||||
</joint>
|
||||
|
||||
<!-- Camera -->
|
||||
<link name="camera_link">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
<material name="red">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<inertial>
|
||||
<mass value="1e-2" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<!-- camera -->
|
||||
|
||||
|
||||
|
||||
<!-- Hokuyo joint -->
|
||||
<joint name="hokuyo_joint" type="fixed">
|
||||
<origin xyz="0.125 0.0 0.225" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<axis xyz="0 1 0" />
|
||||
<child link="laser_frame"/>
|
||||
</joint>
|
||||
|
||||
<!-- Hokuyo Laser -->
|
||||
<link name="laser_frame">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.05 0.05 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="/home/hshine/plan_control_ws/src/plc_bringup/meshes/hokuyo.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
||||
<inertial>
|
||||
<mass value="1e-2" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<!-- hokuyo -->
|
||||
|
||||
<!-- gazebo插件设置相关 -->
|
||||
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Orange</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="wheel_1">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="wheel_2">
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="wheel_3">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="wheel_4">
|
||||
<material>Gazebo/Black</material>
|
||||
</gazebo>
|
||||
|
||||
|
||||
<!-- ros_control plugin -->
|
||||
<!-- <gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/robot</robotNamespace>
|
||||
<legacyModeNS> true </legacyModeNS>
|
||||
</plugin>
|
||||
</gazebo> -->
|
||||
|
||||
|
||||
|
||||
<!-- camera_link -->
|
||||
<gazebo reference="camera_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
|
||||
<!-- hokuyo -->
|
||||
<gazebo reference="laser_frame">
|
||||
<sensor type="ray" name="head_hokuyo_sensor">
|
||||
<!-- <sensor type="gpu_ray" name="head_hokuyo_sensor"> -->
|
||||
<!-- GPU -->
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<update_rate>15</update_rate>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>720</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>-3.1415926</min_angle>
|
||||
<max_angle>3.1415926</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.05</min>
|
||||
<max>30.0</max>
|
||||
<resolution>0.01</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise parameters based on published spec for Hokuyo laser
|
||||
achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
|
||||
stddev of 0.01m will put 99.7% of samples within 0.03m of the true
|
||||
reading. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<!-- GPU版本 -->
|
||||
<!-- <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so"> -->
|
||||
<!-- 换成TURTLEBOT的激光雷达 -->
|
||||
<plugin name="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frameName>laser_frame</frameName>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
<!-- camera -->
|
||||
<gazebo reference="camera_link">
|
||||
<sensor type="camera" name="camera1">
|
||||
<update_rate>30.0</update_rate>
|
||||
<camera name="head">
|
||||
<horizontal_fov>1.3962634</horizontal_fov>
|
||||
<image>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<!-- Noise is sampled independently per pixel on each frame.
|
||||
That pixel's noise value is added to each of its color
|
||||
channels, which at that point lie in the range [0,1]. -->
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>0.0</updateRate>
|
||||
<cameraName>/</cameraName>
|
||||
<imageTopicName>cam</imageTopicName>
|
||||
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
|
||||
<frameName>camera_link</frameName>
|
||||
<hackBaseline>0.07</hackBaseline>
|
||||
<distortionK1>0.0</distortionK1>
|
||||
<distortionK2>0.0</distortionK2>
|
||||
<distortionK3>0.0</distortionK3>
|
||||
<distortionT1>0.0</distortionT1>
|
||||
<distortionT2>0.0</distortionT2>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
<!-- Drive controller -->
|
||||
<gazebo>
|
||||
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>100.0</updateRate>
|
||||
<robotNamespace>/</robotNamespace>
|
||||
<leftFrontJoint>base_to_wheel1</leftFrontJoint>
|
||||
<rightFrontJoint>base_to_wheel3</rightFrontJoint>
|
||||
<leftRearJoint>base_to_wheel2</leftRearJoint>
|
||||
<rightRearJoint>base_to_wheel4</rightRearJoint>
|
||||
<wheelSeparation>0.26</wheelSeparation>
|
||||
<wheelDiameter>0.12</wheelDiameter>
|
||||
<torque>2</torque>
|
||||
<commandTopic>cmd_vel</commandTopic>
|
||||
<odometryTopic>odom</odometryTopic>
|
||||
<odometryFrame>odom</odometryFrame>
|
||||
<robotBaseFrame>dummy</robotBaseFrame>
|
||||
<broadcastTF>1</broadcastTF>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<!-- imu控制 -->
|
||||
|
||||
<!-- <gazebo reference="imu_link">
|
||||
<material>Gazebo/Orange</material>
|
||||
<gravity>true</gravity>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>100</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<topic>__default_topic__</topic>
|
||||
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
|
||||
<topicName>imu</topicName>
|
||||
<bodyName>imu_link</bodyName>
|
||||
<updateRateHZ>100.0</updateRateHZ>
|
||||
<gaussianNoise>0.0</gaussianNoise>
|
||||
<xyzOffset>0 0 0</xyzOffset>
|
||||
<rpyOffset>0 0 0</rpyOffset>
|
||||
<frameName>imu_link</frameName>
|
||||
</plugin>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
</sensor>
|
||||
</gazebo> -->
|
||||
|
||||
<!-- gazebo插件设置相关结束 -->
|
||||
|
||||
</robot>
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,504 @@
|
|||
<?xml version="1.0"?>
|
||||
<sdf version="1.6">
|
||||
<model name="turtlebot3_waffle">
|
||||
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
|
||||
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<link name="base_link">
|
||||
|
||||
<inertial>
|
||||
<pose>-0.064 0 0.048 0 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>1.0</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="base_collision">
|
||||
<pose>-0.064 0 0.048 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.265 0.265 0.089</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="base_visual">
|
||||
<pose>-0.064 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_common/meshes/waffle_base.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="imu_link">
|
||||
<sensor name="tb3_imu" type="imu">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>200</update_rate>
|
||||
<imu>
|
||||
<angular_velocity>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</angular_velocity>
|
||||
<linear_acceleration>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</linear_acceleration>
|
||||
</imu>
|
||||
<plugin name="turtlebot3_imu" filename="libgazebo_ros_imu_sensor.so">
|
||||
<initial_orientation_as_reference>false</initial_orientation_as_reference>
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=imu</remapping>
|
||||
</ros>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="base_scan">
|
||||
<inertial>
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>0.125</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="lidar_sensor_collision">
|
||||
<pose>-0.052 0 0.111 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.0508</radius>
|
||||
<length>0.055</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="lidar_sensor_visual">
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_common/meshes/lds.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
||||
<sensor name="hls_lfcd_lds" type="ray">
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<pose>-0.064 0 0.15 0 0 0</pose>
|
||||
<update_rate>5</update_rate>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>360</samples>
|
||||
<resolution>1.000000</resolution>
|
||||
<min_angle>0.000000</min_angle>
|
||||
<max_angle>6.280000</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.120000</min>
|
||||
<max>20.0</max>
|
||||
<resolution>0.015000</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<plugin name="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frame_name>base_scan</frame_name>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="wheel_left_link">
|
||||
|
||||
<inertial>
|
||||
<pose>0.0 0.144 0.023 -1.57 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>0.1</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="wheel_left_collision">
|
||||
<pose>0.0 0.144 0.023 -1.57 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.033</radius>
|
||||
<length>0.018</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<!-- This friction pamareter don't contain reliable data!! -->
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100000.0</mu>
|
||||
<mu2>100000.0</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0.0</slip1>
|
||||
<slip2>0.0</slip2>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+5</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name="wheel_left_visual">
|
||||
<pose>0.0 0.144 0.023 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_common/meshes/tire.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="wheel_right_link">
|
||||
|
||||
<inertial>
|
||||
<pose>0.0 -0.144 0.023 -1.57 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>0.1</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="wheel_right_collision">
|
||||
<pose>0.0 -0.144 0.023 -1.57 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.033</radius>
|
||||
<length>0.018</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<!-- This friction pamareter don't contain reliable data!! -->
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100000.0</mu>
|
||||
<mu2>100000.0</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0.0</slip1>
|
||||
<slip2>0.0</slip2>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+5</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name="wheel_right_visual">
|
||||
<pose>0.0 -0.144 0.023 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_common/meshes/tire.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name='caster_back_right_link'>
|
||||
<pose>-0.177 -0.064 -0.004 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.001</mass>
|
||||
<inertia>
|
||||
<ixx>0.00001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.00001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.00001</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='collision'>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.005000</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+5</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name='caster_back_left_link'>
|
||||
<pose>-0.177 0.064 -0.004 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.001</mass>
|
||||
<inertia>
|
||||
<ixx>0.00001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.00001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.00001</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='collision'>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.005000</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+5</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="camera_link">
|
||||
<inertial>
|
||||
<pose>0.069 -0.047 0.107 0 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>0.035</mass>
|
||||
</inertial>
|
||||
<collision name="collision">
|
||||
<pose>0 0.047 -0.005 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.008 0.130 0.022</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<pose>0.069 -0.047 0.107 0 0 0</pose>
|
||||
|
||||
<sensor name="intel_realsense_r200_depth" type="depth">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>5</update_rate>
|
||||
<pose>0.064 -0.047 0.107 0 0 0</pose>
|
||||
<camera name="realsense_depth_camera">
|
||||
</camera>
|
||||
<plugin name="intel_realsense_r200_depth_driver" filename="libgazebo_ros_camera.so">
|
||||
<ros>
|
||||
</ros>
|
||||
<camera_name>intel_realsense_r200_depth</camera_name>
|
||||
<frame_name>camera_depth_frame</frame_name>
|
||||
<hack_baseline>0.07</hack_baseline>
|
||||
<min_depth>0.001</min_depth>
|
||||
<max_depth>5.0</max_depth>
|
||||
</plugin>
|
||||
</sensor>
|
||||
|
||||
<collision name="collision">
|
||||
<pose>0 0.047 -0.005 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.008 0.130 0.022</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<pose>0.069 -0.047 0.107 0 0 0</pose>
|
||||
</link>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent>base_footprint</parent>
|
||||
<child>base_link</child>
|
||||
<pose>0.0 0.0 0.010 0 0 0</pose>
|
||||
</joint>
|
||||
|
||||
<joint name="wheel_left_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>wheel_left_link</child>
|
||||
<pose>0.0 0.144 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="wheel_right_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>wheel_right_link</child>
|
||||
<pose>0.0 -0.144 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_right_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_right_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_left_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_left_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>base_scan</child>
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>camera_link</child>
|
||||
<pose>0.064 -0.065 0.094 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
|
||||
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<!--since gazebo_plugins publish tf topic in global namespace /tf only and it
|
||||
cannot be remapped like (namespace/tf) through launch arguments -->
|
||||
<remapping>/tf:=tf</remapping>
|
||||
</ros>
|
||||
|
||||
<update_rate>30</update_rate>
|
||||
|
||||
<!-- wheels -->
|
||||
<left_joint>wheel_left_joint</left_joint>
|
||||
<right_joint>wheel_right_joint</right_joint>
|
||||
|
||||
<!-- kinematics -->
|
||||
<wheel_separation>0.287</wheel_separation>
|
||||
<wheel_diameter>0.066</wheel_diameter>
|
||||
|
||||
<!-- limits -->
|
||||
<max_wheel_torque>20</max_wheel_torque>
|
||||
<max_wheel_acceleration>1.0</max_wheel_acceleration>
|
||||
|
||||
<command_topic>cmd_vel</command_topic>
|
||||
|
||||
<!-- output -->
|
||||
<publish_odom>true</publish_odom>
|
||||
<publish_odom_tf>true</publish_odom_tf>
|
||||
<publish_wheel_tf>false</publish_wheel_tf>
|
||||
|
||||
<odometry_topic>odom</odometry_topic>
|
||||
<odometry_frame>odom</odometry_frame>
|
||||
<robot_base_frame>base_footprint</robot_base_frame>
|
||||
|
||||
</plugin>
|
||||
|
||||
<plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=joint_states</remapping>
|
||||
</ros>
|
||||
<update_rate>30</update_rate>
|
||||
<joint_name>wheel_left_joint</joint_name>
|
||||
<joint_name>wheel_right_joint</joint_name>
|
||||
</plugin>
|
||||
|
||||
</model>
|
||||
</sdf>
|
||||
|
|
@ -0,0 +1,504 @@
|
|||
<?xml version="1.0"?>
|
||||
<sdf version="1.6">
|
||||
<model name="turtlebot3_waffle">
|
||||
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
|
||||
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<link name="base_link">
|
||||
|
||||
<inertial>
|
||||
<pose>-0.064 0 0.048 0 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>1.0</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="base_collision">
|
||||
<pose>-0.064 0 0.048 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.265 0.265 0.089</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="base_visual">
|
||||
<pose>-0.064 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_common/meshes/waffle_base.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="imu_link">
|
||||
<sensor name="tb3_imu" type="imu">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>200</update_rate>
|
||||
<imu>
|
||||
<angular_velocity>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</angular_velocity>
|
||||
<linear_acceleration>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</linear_acceleration>
|
||||
</imu>
|
||||
<plugin name="turtlebot3_imu" filename="libgazebo_ros_imu_sensor.so">
|
||||
<initial_orientation_as_reference>false</initial_orientation_as_reference>
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=imu</remapping>
|
||||
</ros>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="base_scan">
|
||||
<inertial>
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>0.125</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="lidar_sensor_collision">
|
||||
<pose>-0.052 0 0.111 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.0508</radius>
|
||||
<length>0.055</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="lidar_sensor_visual">
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_common/meshes/lds.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
||||
<sensor name="hls_lfcd_lds" type="ray">
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<pose>-0.064 0 0.15 0 0 0</pose>
|
||||
<update_rate>5</update_rate>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>360</samples>
|
||||
<resolution>1.000000</resolution>
|
||||
<min_angle>0.000000</min_angle>
|
||||
<max_angle>6.280000</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.120000</min>
|
||||
<max>20.0</max>
|
||||
<resolution>0.015000</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<plugin name="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=scan</remapping>
|
||||
</ros>
|
||||
<output_type>sensor_msgs/LaserScan</output_type>
|
||||
<frame_name>base_scan</frame_name>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<link name="wheel_left_link">
|
||||
|
||||
<inertial>
|
||||
<pose>0.0 0.144 0.023 -1.57 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>0.1</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="wheel_left_collision">
|
||||
<pose>0.0 0.144 0.023 -1.57 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.033</radius>
|
||||
<length>0.018</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<!-- This friction pamareter don't contain reliable data!! -->
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100000.0</mu>
|
||||
<mu2>100000.0</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0.0</slip1>
|
||||
<slip2>0.0</slip2>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+5</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name="wheel_left_visual">
|
||||
<pose>0.0 0.144 0.023 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_common/meshes/tire.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="wheel_right_link">
|
||||
|
||||
<inertial>
|
||||
<pose>0.0 -0.144 0.023 -1.57 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>0.1</mass>
|
||||
</inertial>
|
||||
|
||||
<collision name="wheel_right_collision">
|
||||
<pose>0.0 -0.144 0.023 -1.57 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.033</radius>
|
||||
<length>0.018</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<!-- This friction pamareter don't contain reliable data!! -->
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100000.0</mu>
|
||||
<mu2>100000.0</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0.0</slip1>
|
||||
<slip2>0.0</slip2>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+5</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
|
||||
<visual name="wheel_right_visual">
|
||||
<pose>0.0 -0.144 0.023 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://turtlebot3_common/meshes/tire.dae</uri>
|
||||
<scale>0.001 0.001 0.001</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name='caster_back_right_link'>
|
||||
<pose>-0.177 -0.064 -0.004 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.001</mass>
|
||||
<inertia>
|
||||
<ixx>0.00001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.00001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.00001</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='collision'>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.005000</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+5</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name='caster_back_left_link'>
|
||||
<pose>-0.177 0.064 -0.004 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.001</mass>
|
||||
<inertia>
|
||||
<ixx>0.00001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.00001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.00001</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='collision'>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.005000</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+5</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="camera_link">
|
||||
<inertial>
|
||||
<pose>0.069 -0.047 0.107 0 0 0</pose>
|
||||
<inertia>
|
||||
<ixx>0.001</ixx>
|
||||
<ixy>0.000</ixy>
|
||||
<ixz>0.000</ixz>
|
||||
<iyy>0.001</iyy>
|
||||
<iyz>0.000</iyz>
|
||||
<izz>0.001</izz>
|
||||
</inertia>
|
||||
<mass>0.035</mass>
|
||||
</inertial>
|
||||
<collision name="collision">
|
||||
<pose>0 0.047 -0.005 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.008 0.130 0.022</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<pose>0.069 -0.047 0.107 0 0 0</pose>
|
||||
|
||||
<sensor name="intel_realsense_r200_depth" type="depth">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>5</update_rate>
|
||||
<pose>0.064 -0.047 0.107 0 0 0</pose>
|
||||
<camera name="realsense_depth_camera">
|
||||
</camera>
|
||||
<plugin name="intel_realsense_r200_depth_driver" filename="libgazebo_ros_camera.so">
|
||||
<ros>
|
||||
</ros>
|
||||
<camera_name>intel_realsense_r200_depth</camera_name>
|
||||
<frame_name>camera_depth_frame</frame_name>
|
||||
<hack_baseline>0.07</hack_baseline>
|
||||
<min_depth>0.001</min_depth>
|
||||
<max_depth>5.0</max_depth>
|
||||
</plugin>
|
||||
</sensor>
|
||||
|
||||
<collision name="collision">
|
||||
<pose>0 0.047 -0.005 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.008 0.130 0.022</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<pose>0.069 -0.047 0.107 0 0 0</pose>
|
||||
</link>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent>base_footprint</parent>
|
||||
<child>base_link</child>
|
||||
<pose>0.0 0.0 0.010 0 0 0</pose>
|
||||
</joint>
|
||||
|
||||
<joint name="wheel_left_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>wheel_left_link</child>
|
||||
<pose>0.0 0.144 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="wheel_right_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>wheel_right_link</child>
|
||||
<pose>0.0 -0.144 0.023 -1.57 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_right_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_right_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name='caster_back_left_joint' type='ball'>
|
||||
<parent>base_link</parent>
|
||||
<child>caster_back_left_link</child>
|
||||
</joint>
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>base_scan</child>
|
||||
<pose>-0.064 0 0.121 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>camera_link</child>
|
||||
<pose>0.064 -0.065 0.094 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
|
||||
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<!--since gazebo_plugins publish tf topic in global namespace /tf only and it
|
||||
cannot be remapped like (namespace/tf) through launch arguments -->
|
||||
<remapping>/tf:=tf</remapping>
|
||||
</ros>
|
||||
|
||||
<update_rate>30</update_rate>
|
||||
|
||||
<!-- wheels -->
|
||||
<left_joint>wheel_left_joint</left_joint>
|
||||
<right_joint>wheel_right_joint</right_joint>
|
||||
|
||||
<!-- kinematics -->
|
||||
<wheel_separation>0.287</wheel_separation>
|
||||
<wheel_diameter>0.066</wheel_diameter>
|
||||
|
||||
<!-- limits -->
|
||||
<max_wheel_torque>20</max_wheel_torque>
|
||||
<max_wheel_acceleration>1.0</max_wheel_acceleration>
|
||||
|
||||
<command_topic>cmd_vel</command_topic>
|
||||
|
||||
<!-- output -->
|
||||
<publish_odom>true</publish_odom>
|
||||
<publish_odom_tf>true</publish_odom_tf>
|
||||
<publish_wheel_tf>false</publish_wheel_tf>
|
||||
|
||||
<odometry_topic>odom</odometry_topic>
|
||||
<odometry_frame>odom</odometry_frame>
|
||||
<robot_base_frame>base_footprint</robot_base_frame>
|
||||
|
||||
</plugin>
|
||||
|
||||
<plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<remapping>~/out:=joint_states</remapping>
|
||||
</ros>
|
||||
<update_rate>30</update_rate>
|
||||
<joint_name>wheel_left_joint</joint_name>
|
||||
<joint_name>wheel_right_joint</joint_name>
|
||||
</plugin>
|
||||
|
||||
</model>
|
||||
</sdf>
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
<?xml version="1.0"?>
|
||||
<sdf version="1.6">
|
||||
<world name="default">
|
||||
|
||||
<include>
|
||||
<uri>model://ground_plane</uri>
|
||||
</include>
|
||||
|
||||
<include>
|
||||
<uri>model://sun</uri>
|
||||
</include>
|
||||
|
||||
<scene>
|
||||
<shadows>false</shadows>
|
||||
</scene>
|
||||
|
||||
<gui fullscreen='0'>
|
||||
<camera name='user_camera'>
|
||||
<pose frame=''>0 0 10 0 1.570796 0</pose>
|
||||
<view_controller>orbit</view_controller>
|
||||
<projection_type>perspective</projection_type>
|
||||
</camera>
|
||||
</gui>
|
||||
|
||||
<physics type="ode">
|
||||
<real_time_update_rate>1000.0</real_time_update_rate>
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1</real_time_factor>
|
||||
<ode>
|
||||
<solver>
|
||||
<type>quick</type>
|
||||
<iters>150</iters>
|
||||
<precon_iters>0</precon_iters>
|
||||
<sor>1.400000</sor>
|
||||
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
|
||||
</solver>
|
||||
<constraints>
|
||||
<cfm>0.00001</cfm>
|
||||
<erp>0.2</erp>
|
||||
<contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
|
||||
<contact_surface_layer>0.01000</contact_surface_layer>
|
||||
</constraints>
|
||||
</ode>
|
||||
</physics>
|
||||
|
||||
<model name="turtlebot3_world">
|
||||
<static>1</static>
|
||||
<include>
|
||||
<uri>model://turtlebot3_world</uri>
|
||||
</include>
|
||||
</model>
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
Loading…
Reference in New Issue