add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,123 @@
|
||||
# Nav2 Simple (Python3) Commander
|
||||
|
||||
## Overview
|
||||
|
||||
The goal of this package is to provide a "navigation as a library" capability to Python3 users. We provide an API that handles all the ROS2-y and Action Server-y things for you such that you can focus on building an application leveraging the capabilities of Nav2. We also provide you with demos and examples of API usage to build common basic capabilities in autonomous mobile robotics.
|
||||
|
||||
This was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) at [Samsung Research](https://www.sra.samsung.com/), with initial prototypes being prepared for the Keynote at the [2021 ROS Developers Day](https://www.theconstructsim.com/ros-developers-day-2021/) conference (code can be found [here](https://github.com/SteveMacenski/nav2_rosdevday_2021)).
|
||||
|
||||
|
||||
|
||||
## API
|
||||
|
||||
See its [API Guide Page](https://navigation.ros.org/commander_api/index.html) for additional parameter descriptions.
|
||||
|
||||
The methods provided by the basic navigator are shown below, with inputs and expected returns. If a server fails, it may throw an exception or return a `None` object, so please be sure to properly wrap your navigation calls in try/catch and check results for `None` type.
|
||||
|
||||
New as of September 2023: the simple navigator constructor will accept a `namespace` field to support multi-robot applications or namespaced Nav2 launches.
|
||||
|
||||
| Robot Navigator Method | Description |
|
||||
| --------------------------------- | -------------------------------------------------------------------------- |
|
||||
| setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. |
|
||||
| goThroughPoses(poses, behavior_tree='') | Requests the robot to drive through a set of poses (list of `PoseStamped`).|
|
||||
| goToPose(pose, behavior_tree='') | Requests the robot to drive to a pose (`PoseStamped`). |
|
||||
| followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. |
|
||||
| followPath(path, controller_id='', goal_checker_id='') | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. |
|
||||
| spin(spin_dist=1.57, time_allowance=10) | Requests the robot to performs an in-place rotation by a given angle. |
|
||||
| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10) | Requests the robot to back up by a given distance. |
|
||||
| cancelTask() | Cancel an ongoing task request.|
|
||||
| isTaskComplete() | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. |
|
||||
| getFeedback() | Gets feedback from task, returns action server feedback object. |
|
||||
| getResult() | Gets final result of task, to be called after `isTaskComplete` returns `True`. Returns action server result object. |
|
||||
| getPath(start, goal, planner_id='', use_start=False) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. |
|
||||
| getPathThroughPoses(start, goals, planner_id='', use_start=False) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. |
|
||||
| smoothPath(path, smoother_id='', max_duration=2.0, check_for_collision=False) | Smooths a given `nav_msgs/msg/Path` path. |
|
||||
| changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. |
|
||||
| clearAllCostmaps() | Clears both the global and local costmaps. |
|
||||
| clearLocalCostmap() | Clears the local costmap. |
|
||||
| clearGlobalCostmap() | Clears the global costmap. |
|
||||
| getGlobalCostmap() | Returns the global costmap, `nav2_msgs/Costmap` |
|
||||
| getLocalCostmap() | Returns the local costmap, `nav2_msgs/Costmap` |
|
||||
| waitUntilNav2Active(navigator='bt_navigator, localizer='amcl') | Blocks until Nav2 is completely online and lifecycle nodes are in the active state. To be used in conjunction with autostart or external lifecycle bringup. Custom navigator and localizer nodes can be specified |
|
||||
| lifecycleStartup() | Sends a request to all lifecycle management servers to bring them into the active state, to be used if autostart is `false` and you want this program to control Nav2's lifecycle. |
|
||||
| lifecycleShutdown() | Sends a request to all lifecycle management servers to shut them down. |
|
||||
| destroyNode() | Releases the resources used by the object. |
|
||||
|
||||
A general template for building applications is as follows:
|
||||
|
||||
``` python3
|
||||
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator
|
||||
import rclpy
|
||||
|
||||
rclpy.init()
|
||||
|
||||
nav = BasicNavigator()
|
||||
...
|
||||
nav.setInitialPose(init_pose)
|
||||
nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()`
|
||||
...
|
||||
path = nav.getPath(init_pose, goal_pose)
|
||||
smoothed_path = nav.smoothPath(path)
|
||||
...
|
||||
nav.goToPose(goal_pose)
|
||||
while not nav.isTaskComplete():
|
||||
feedback = nav.getFeedback()
|
||||
if feedback.navigation_duration > 600:
|
||||
nav.cancelTask()
|
||||
...
|
||||
result = nav.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Goal succeeded!')
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Goal was canceled!')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Goal failed!')
|
||||
```
|
||||
|
||||
## Usage of Demos and Examples
|
||||
|
||||
Make sure to install the `aws_robomaker_small_warehouse_world` package or build it in your local workspace alongside Nav2. It can be found [here](https://github.com/aws-robotics/aws-robomaker-small-warehouse-world). The demonstrations, examples, and launch files assume you're working with this gazebo world (such that the hard-programmed shelf locations and routes highlighting the API are meaningful).
|
||||
|
||||
Make sure you have set the model directory of turtlebot3 simulation and aws warehouse world to the `GAZEBO_MODEL_PATH`. There are 2 main ways to run the demos of the `nav2_simple_commander` API.
|
||||
|
||||
### Automatically
|
||||
|
||||
The main benefit of this is automatically showing the above demonstrations in a single command for the default robot model and world. This will make use of Nav2's default robot and parameters set out in the main simulation launch file in `nav2_bringup`.
|
||||
|
||||
``` bash
|
||||
# Launch the launch file for the demo / example
|
||||
ros2 launch nav2_simple_commander security_demo_launch.py
|
||||
```
|
||||
|
||||
This will bring up the robot in the AWS Warehouse in a reasonable position, launch the autonomy script, and complete some task to demonstrate the `nav2_simple_commander` API.
|
||||
|
||||
### Manually
|
||||
|
||||
The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstration. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment.
|
||||
|
||||
``` bash
|
||||
# Terminal 1: launch your robot navigation and simulation (or physical robot). For example
|
||||
ros2 launch nav2_bringup tb3_simulation_launch.py world:=/path/to/aws_robomaker_small_warehouse_world/.world map:=/path/to/aws_robomaker_small_warehouse_world/.yaml
|
||||
|
||||
# Terminal 2: launch your autonomy / application demo or example. For example
|
||||
ros2 run nav2_simple_commander demo_security
|
||||
```
|
||||
|
||||
Then you should see the autonomy application running!
|
||||
|
||||
## Examples
|
||||
|
||||
The `nav2_simple_commander` has a few examples to highlight the API functions available to you as a user:
|
||||
|
||||
- `example_nav_to_pose.py` - Demonstrates the navigate to pose capabilities of the navigator, as well as a number of auxiliary methods.
|
||||
- `example_nav_through_poses.py` - Demonstrates the navigate through poses capabilities of the navigator, as well as a number of auxiliary methods.
|
||||
- `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods.
|
||||
- `example_follow_path.py` - Demonstrates the path following capabilities of the navigator, as well as a number of auxiliary methods such as path smoothing.
|
||||
## Demos
|
||||
|
||||
The `nav2_simple_commander` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the `nav2_simple_commander` API:
|
||||
|
||||
- `demo_security.py` - A simple security robot application, showing how to have a robot follow a security route using Navigate Through Poses to do a patrol route, indefinitely.
|
||||
- `demo_picking.py` - A simple item picking application, showing how to have a robot drive to a specific shelf in a warehouse to either pick an item or have a person place an item into a basket and deliver it to a destination for shipping using Navigate To Pose.
|
||||
- `demo_inspection.py` - A simple shelf inspection application, showing how to use the Waypoint Follower and task executors to take pictures, RFID scans, etc of shelves to analyze the current shelf statuses and locate items in the warehouse.
|
||||
@@ -0,0 +1,99 @@
|
||||
# Copyright (c) 2021 Samsung Research America
|
||||
# Copyright (c) 2022 Joshua Wallace
|
||||
#
|
||||
# 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, 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():
|
||||
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
python_commander_dir = get_package_share_directory('nav2_simple_commander')
|
||||
|
||||
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
|
||||
world = os.path.join(python_commander_dir, 'warehouse.world')
|
||||
|
||||
# Launch configuration variables
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
headless = LaunchConfiguration('headless')
|
||||
|
||||
# Declare the launch arguments
|
||||
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)')
|
||||
|
||||
# start the simulation
|
||||
start_gazebo_server_cmd = ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
start_gazebo_client_cmd = ExecuteProcess(
|
||||
condition=IfCondition(PythonExpression(['not ', headless])),
|
||||
cmd=['gzclient'],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
arguments=[urdf])
|
||||
|
||||
# start the visualization
|
||||
rviz_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
condition=IfCondition(use_rviz),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
# start navigation
|
||||
bringup_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'map': map_yaml_file}.items())
|
||||
|
||||
# start the demo autonomy task
|
||||
demo_cmd = Node(
|
||||
package='nav2_simple_commander',
|
||||
executable='example_assisted_teleop',
|
||||
emulate_tty=True,
|
||||
output='screen')
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(start_gazebo_server_cmd)
|
||||
ld.add_action(start_gazebo_client_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
ld.add_action(bringup_cmd)
|
||||
ld.add_action(demo_cmd)
|
||||
return ld
|
||||
@@ -0,0 +1,98 @@
|
||||
# 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.
|
||||
|
||||
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():
|
||||
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
python_commander_dir = get_package_share_directory('nav2_simple_commander')
|
||||
|
||||
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
|
||||
world = os.path.join(python_commander_dir, 'warehouse.world')
|
||||
|
||||
# Launch configuration variables
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
headless = LaunchConfiguration('headless')
|
||||
|
||||
# Declare the launch arguments
|
||||
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)')
|
||||
|
||||
# start the simulation
|
||||
start_gazebo_server_cmd = ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
start_gazebo_client_cmd = ExecuteProcess(
|
||||
condition=IfCondition(PythonExpression(['not ', headless])),
|
||||
cmd=['gzclient'],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
arguments=[urdf])
|
||||
|
||||
# start the visualization
|
||||
rviz_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
condition=IfCondition(use_rviz),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
# start navigation
|
||||
bringup_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'map': map_yaml_file}.items())
|
||||
|
||||
# start the demo autonomy task
|
||||
demo_cmd = Node(
|
||||
package='nav2_simple_commander',
|
||||
executable='example_follow_path',
|
||||
emulate_tty=True,
|
||||
output='screen')
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(start_gazebo_server_cmd)
|
||||
ld.add_action(start_gazebo_client_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
ld.add_action(bringup_cmd)
|
||||
ld.add_action(demo_cmd)
|
||||
return ld
|
||||
@@ -0,0 +1,98 @@
|
||||
# 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.
|
||||
|
||||
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():
|
||||
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
python_commander_dir = get_package_share_directory('nav2_simple_commander')
|
||||
|
||||
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
|
||||
world = os.path.join(python_commander_dir, 'warehouse.world')
|
||||
|
||||
# Launch configuration variables
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
headless = LaunchConfiguration('headless')
|
||||
|
||||
# Declare the launch arguments
|
||||
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)')
|
||||
|
||||
# start the simulation
|
||||
start_gazebo_server_cmd = ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
start_gazebo_client_cmd = ExecuteProcess(
|
||||
condition=IfCondition(PythonExpression(['not ', headless])),
|
||||
cmd=['gzclient'],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
arguments=[urdf])
|
||||
|
||||
# start the visualization
|
||||
rviz_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
condition=IfCondition(use_rviz),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
# start navigation
|
||||
bringup_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'map': map_yaml_file}.items())
|
||||
|
||||
# start the demo autonomy task
|
||||
demo_cmd = Node(
|
||||
package='nav2_simple_commander',
|
||||
executable='demo_inspection',
|
||||
emulate_tty=True,
|
||||
output='screen')
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(start_gazebo_server_cmd)
|
||||
ld.add_action(start_gazebo_client_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
ld.add_action(bringup_cmd)
|
||||
ld.add_action(demo_cmd)
|
||||
return ld
|
||||
@@ -0,0 +1,98 @@
|
||||
# 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.
|
||||
|
||||
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():
|
||||
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
python_commander_dir = get_package_share_directory('nav2_simple_commander')
|
||||
|
||||
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
|
||||
world = os.path.join(python_commander_dir, 'warehouse.world')
|
||||
|
||||
# Launch configuration variables
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
headless = LaunchConfiguration('headless')
|
||||
|
||||
# Declare the launch arguments
|
||||
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)')
|
||||
|
||||
# start the simulation
|
||||
start_gazebo_server_cmd = ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
start_gazebo_client_cmd = ExecuteProcess(
|
||||
condition=IfCondition(PythonExpression(['not ', headless])),
|
||||
cmd=['gzclient'],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
arguments=[urdf])
|
||||
|
||||
# start the visualization
|
||||
rviz_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
condition=IfCondition(use_rviz),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
# start navigation
|
||||
bringup_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'map': map_yaml_file}.items())
|
||||
|
||||
# start the demo autonomy task
|
||||
demo_cmd = Node(
|
||||
package='nav2_simple_commander',
|
||||
executable='example_nav_through_poses',
|
||||
emulate_tty=True,
|
||||
output='screen')
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(start_gazebo_server_cmd)
|
||||
ld.add_action(start_gazebo_client_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
ld.add_action(bringup_cmd)
|
||||
ld.add_action(demo_cmd)
|
||||
return ld
|
||||
@@ -0,0 +1,98 @@
|
||||
# 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.
|
||||
|
||||
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():
|
||||
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
python_commander_dir = get_package_share_directory('nav2_simple_commander')
|
||||
|
||||
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
|
||||
world = os.path.join(python_commander_dir, 'warehouse.world')
|
||||
|
||||
# Launch configuration variables
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
headless = LaunchConfiguration('headless')
|
||||
|
||||
# Declare the launch arguments
|
||||
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)')
|
||||
|
||||
# start the simulation
|
||||
start_gazebo_server_cmd = ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
start_gazebo_client_cmd = ExecuteProcess(
|
||||
condition=IfCondition(PythonExpression(['not ', headless])),
|
||||
cmd=['gzclient'],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
arguments=[urdf])
|
||||
|
||||
# start the visualization
|
||||
rviz_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
condition=IfCondition(use_rviz),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
# start navigation
|
||||
bringup_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'map': map_yaml_file}.items())
|
||||
|
||||
# start the demo autonomy task
|
||||
demo_cmd = Node(
|
||||
package='nav2_simple_commander',
|
||||
executable='example_nav_to_pose',
|
||||
emulate_tty=True,
|
||||
output='screen')
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(start_gazebo_server_cmd)
|
||||
ld.add_action(start_gazebo_client_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
ld.add_action(bringup_cmd)
|
||||
ld.add_action(demo_cmd)
|
||||
return ld
|
||||
@@ -0,0 +1,98 @@
|
||||
# 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.
|
||||
|
||||
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():
|
||||
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
python_commander_dir = get_package_share_directory('nav2_simple_commander')
|
||||
|
||||
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
|
||||
world = os.path.join(python_commander_dir, 'warehouse.world')
|
||||
|
||||
# Launch configuration variables
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
headless = LaunchConfiguration('headless')
|
||||
|
||||
# Declare the launch arguments
|
||||
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)')
|
||||
|
||||
# start the simulation
|
||||
start_gazebo_server_cmd = ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
start_gazebo_client_cmd = ExecuteProcess(
|
||||
condition=IfCondition(PythonExpression(['not ', headless])),
|
||||
cmd=['gzclient'],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
arguments=[urdf])
|
||||
|
||||
# start the visualization
|
||||
rviz_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
condition=IfCondition(use_rviz),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
# start navigation
|
||||
bringup_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'map': map_yaml_file}.items())
|
||||
|
||||
# start the demo autonomy task
|
||||
demo_cmd = Node(
|
||||
package='nav2_simple_commander',
|
||||
executable='demo_picking',
|
||||
emulate_tty=True,
|
||||
output='screen')
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(start_gazebo_server_cmd)
|
||||
ld.add_action(start_gazebo_client_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
ld.add_action(bringup_cmd)
|
||||
ld.add_action(demo_cmd)
|
||||
return ld
|
||||
@@ -0,0 +1,98 @@
|
||||
# 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.
|
||||
|
||||
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():
|
||||
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
python_commander_dir = get_package_share_directory('nav2_simple_commander')
|
||||
|
||||
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
|
||||
world = os.path.join(python_commander_dir, 'warehouse.world')
|
||||
|
||||
# Launch configuration variables
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
headless = LaunchConfiguration('headless')
|
||||
|
||||
# Declare the launch arguments
|
||||
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)')
|
||||
|
||||
# start the simulation
|
||||
start_gazebo_server_cmd = ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
start_gazebo_client_cmd = ExecuteProcess(
|
||||
condition=IfCondition(PythonExpression(['not ', headless])),
|
||||
cmd=['gzclient'],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
arguments=[urdf])
|
||||
|
||||
# start the visualization
|
||||
rviz_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
condition=IfCondition(use_rviz),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
# start navigation
|
||||
bringup_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'map': map_yaml_file}.items())
|
||||
|
||||
# start the demo autonomy task
|
||||
demo_cmd = Node(
|
||||
package='nav2_simple_commander',
|
||||
executable='demo_recoveries',
|
||||
emulate_tty=True,
|
||||
output='screen')
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(start_gazebo_server_cmd)
|
||||
ld.add_action(start_gazebo_client_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
ld.add_action(bringup_cmd)
|
||||
ld.add_action(demo_cmd)
|
||||
return ld
|
||||
@@ -0,0 +1,98 @@
|
||||
# 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.
|
||||
|
||||
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():
|
||||
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
python_commander_dir = get_package_share_directory('nav2_simple_commander')
|
||||
|
||||
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
|
||||
world = os.path.join(python_commander_dir, 'warehouse.world')
|
||||
|
||||
# Launch configuration variables
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
headless = LaunchConfiguration('headless')
|
||||
|
||||
# Declare the launch arguments
|
||||
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)')
|
||||
|
||||
# start the simulation
|
||||
start_gazebo_server_cmd = ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
start_gazebo_client_cmd = ExecuteProcess(
|
||||
condition=IfCondition(PythonExpression(['not ', headless])),
|
||||
cmd=['gzclient'],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
arguments=[urdf])
|
||||
|
||||
# start the visualization
|
||||
rviz_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
condition=IfCondition(use_rviz),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
# start navigation
|
||||
bringup_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'map': map_yaml_file}.items())
|
||||
|
||||
# start the demo autonomy task
|
||||
demo_cmd = Node(
|
||||
package='nav2_simple_commander',
|
||||
executable='demo_security',
|
||||
emulate_tty=True,
|
||||
output='screen')
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(start_gazebo_server_cmd)
|
||||
ld.add_action(start_gazebo_client_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
ld.add_action(bringup_cmd)
|
||||
ld.add_action(demo_cmd)
|
||||
return ld
|
||||
@@ -0,0 +1,683 @@
|
||||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<sdf version="1.6">
|
||||
<world name="default">
|
||||
|
||||
<gravity>0 0 -9.8</gravity>
|
||||
<physics default="0" name="default_physics" type="ode">
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1</real_time_factor>
|
||||
<real_time_update_rate>1000</real_time_update_rate>
|
||||
</physics>
|
||||
|
||||
|
||||
<!--model name="aws_robomaker_warehouse_RoofB_01_001">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_RoofB_01</uri>
|
||||
</include>
|
||||
<pose frame="">0.0 0.0 0 0 0 0</pose>
|
||||
</model-->
|
||||
|
||||
<model name="aws_robomaker_warehouse_ShelfF_01_001">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ShelfF_01</uri>
|
||||
</include>
|
||||
<pose frame="">-5.795143 -0.956635 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_WallB_01_001">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_WallB_01</uri>
|
||||
</include>
|
||||
<pose frame="">0.0 0.0 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ShelfE_01_001">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ShelfE_01</uri>
|
||||
</include>
|
||||
<pose frame="">4.73156 0.57943 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ShelfE_01_002">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ShelfE_01</uri>
|
||||
</include>
|
||||
<pose frame="">4.73156 -4.827049 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ShelfE_01_003">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ShelfE_01</uri>
|
||||
</include>
|
||||
<pose frame="">4.73156 -8.6651 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ShelfD_01_001">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ShelfD_01</uri>
|
||||
</include>
|
||||
<pose frame="">4.73156 -1.242668 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ShelfD_01_002">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ShelfD_01</uri>
|
||||
</include>
|
||||
<pose frame="">4.73156 -3.038551 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ShelfD_01_003">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ShelfD_01</uri>
|
||||
</include>
|
||||
<pose frame="">4.73156 -6.750542 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<!--model name="aws_robomaker_warehouse_DeskC_01_001">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_DeskC_01</uri>
|
||||
</include>
|
||||
<pose frame="">-0.061684 6.135864 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_DeskC_01_002">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_DeskC_01</uri>
|
||||
</include>
|
||||
<pose frame="">-0.061684 3.039 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_DeskC_01_003">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_DeskC_01</uri>
|
||||
</include>
|
||||
<pose frame="">-0.061684 -6.6493 0 0 0 0</pose>
|
||||
</model-->
|
||||
|
||||
<model name="aws_robomaker_warehouse_GroundB_01_001">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_GroundB_01</uri>
|
||||
</include>
|
||||
<pose frame="">0.0 0.0 -0.090092 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_Lamp_01_005">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_Lamp_01</uri>
|
||||
</include>
|
||||
<pose frame="">0 0 -4 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
|
||||
|
||||
<model name="aws_robomaker_warehouse_Bucket_01_020">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_Bucket_01</uri>
|
||||
</include>
|
||||
<pose frame="">0.433449 9.631706 0 0 0 -1.563161</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_Bucket_01_021">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_Bucket_01</uri>
|
||||
</include>
|
||||
<pose frame="">-1.8321 -6.3752 0 0 0 -1.563161</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_Bucket_01_022">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_Bucket_01</uri>
|
||||
</include>
|
||||
<pose frame="">0.433449 8.59 0 0 0 -1.563161</pose>
|
||||
</model>
|
||||
|
||||
<model name='aws_robomaker_warehouse_ClutteringA_01_016'>
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringA_01</uri>
|
||||
</include>
|
||||
<pose frame=''>5.708138 8.616844 -0.017477 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name='aws_robomaker_warehouse_ClutteringA_01_017'>
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringA_01</uri>
|
||||
</include>
|
||||
<pose frame=''>3.408638 8.616844 -0.017477 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name='aws_robomaker_warehouse_ClutteringA_01_018'>
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringA_01</uri>
|
||||
</include>
|
||||
<pose frame=''>-1.491287 5.222435 -0.017477 0 0 -1.583185</pose>
|
||||
</model>
|
||||
|
||||
|
||||
<model name="aws_robomaker_warehouse_ClutteringC_01_027">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringC_01</uri>
|
||||
</include>
|
||||
<pose frame="">3.324959 3.822449 -0.012064 0 0 1.563871</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ClutteringC_01_028">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringC_01</uri>
|
||||
</include>
|
||||
<pose frame="">5.54171 3.816475 -0.015663 0 0 -1.583191</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ClutteringC_01_029">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringC_01</uri>
|
||||
</include>
|
||||
<pose frame="">5.384239 6.137154 0 0 0 3.150000</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ClutteringC_01_030">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringC_01</uri>
|
||||
</include>
|
||||
<pose frame="">3.236 6.137154 0 0 0 3.150000</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ClutteringC_01_031">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringC_01</uri>
|
||||
</include>
|
||||
<pose frame="">-1.573677 2.301994 -0.015663 0 0 -3.133191</pose>
|
||||
</model>
|
||||
|
||||
<model name="aws_robomaker_warehouse_ClutteringC_01_032">
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringC_01</uri>
|
||||
</include>
|
||||
<pose frame="">-1.2196 9.407 -0.015663 0 0 1.563871</pose>
|
||||
</model>
|
||||
|
||||
<model name='aws_robomaker_warehouse_ClutteringD_01_005'>
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_ClutteringD_01</uri>
|
||||
</include>
|
||||
<pose frame=''>-1.634682 -7.811813 -0.319559 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
<model name='aws_robomaker_warehouse_TrashCanC_01_002'>
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_TrashCanC_01</uri>
|
||||
</include>
|
||||
<pose frame=''>-1.592441 7.715420 0 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
|
||||
<model name='aws_robomaker_warehouse_PalletJackB_01_001'>
|
||||
<include>
|
||||
<uri>model://aws_robomaker_warehouse_PalletJackB_01</uri>
|
||||
</include>
|
||||
<pose frame=''>-0.276098 -9.481944 0.023266 0 0 0</pose>
|
||||
</model>
|
||||
|
||||
|
||||
<light name="Warehouse_CeilingLight_003" type="point">
|
||||
<pose frame="">0 0 9 0 0 0</pose>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>80</range>
|
||||
<constant>0.3</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
<direction>0.1 0.1 -1</direction>
|
||||
</light>
|
||||
|
||||
<gui fullscreen='0'>
|
||||
<camera name='user_camera'>
|
||||
<pose frame=''>-4.70385 10.895 16.2659 -0 0.921795 -1.12701</pose>
|
||||
<view_controller>orbit</view_controller>
|
||||
<projection_type>perspective</projection_type>
|
||||
</camera>
|
||||
</gui>
|
||||
|
||||
<model name="turtlebot3_waffle">
|
||||
<pose>3.45 2.15 0.01 0.0 0.0 3.14</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.052 0 0.111 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.121 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>3.5</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>
|
||||
|
||||
<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>
|
||||
|
||||
<plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
|
||||
|
||||
<ros>
|
||||
<!-- <namespace>/tb3</namespace> -->
|
||||
<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>
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
@@ -0,0 +1,98 @@
|
||||
# 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.
|
||||
|
||||
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():
|
||||
warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world')
|
||||
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||
python_commander_dir = get_package_share_directory('nav2_simple_commander')
|
||||
|
||||
map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml')
|
||||
world = os.path.join(python_commander_dir, 'warehouse.world')
|
||||
|
||||
# Launch configuration variables
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
headless = LaunchConfiguration('headless')
|
||||
|
||||
# Declare the launch arguments
|
||||
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)')
|
||||
|
||||
# start the simulation
|
||||
start_gazebo_server_cmd = ExecuteProcess(
|
||||
cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
start_gazebo_client_cmd = ExecuteProcess(
|
||||
condition=IfCondition(PythonExpression(['not ', headless])),
|
||||
cmd=['gzclient'],
|
||||
cwd=[warehouse_dir], output='screen')
|
||||
|
||||
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
arguments=[urdf])
|
||||
|
||||
# start the visualization
|
||||
rviz_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
|
||||
condition=IfCondition(use_rviz),
|
||||
launch_arguments={'namespace': '',
|
||||
'use_namespace': 'False'}.items())
|
||||
|
||||
# start navigation
|
||||
bringup_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
|
||||
launch_arguments={'map': map_yaml_file}.items())
|
||||
|
||||
# start the demo autonomy task
|
||||
demo_cmd = Node(
|
||||
package='nav2_simple_commander',
|
||||
executable='example_waypoint_follower',
|
||||
emulate_tty=True,
|
||||
output='screen')
|
||||
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(start_gazebo_server_cmd)
|
||||
ld.add_action(start_gazebo_client_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
ld.add_action(bringup_cmd)
|
||||
ld.add_action(demo_cmd)
|
||||
return ld
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 22 MiB |
@@ -0,0 +1,192 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 2021 Samsung Research America
|
||||
# Copyright 2022 Stevedan Ogochukwu Omodolor
|
||||
# Copyright 2022 Jaehun Jackson Kim
|
||||
#
|
||||
# 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 a Python3 API for costmap 2d messages from the stack.
|
||||
|
||||
It provides the basic conversion, get/set,
|
||||
and handling semantics found in the costmap 2d C++ API.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class PyCostmap2D:
|
||||
"""
|
||||
PyCostmap2D.
|
||||
|
||||
Costmap Python3 API for OccupancyGrids to populate from published messages
|
||||
"""
|
||||
|
||||
def __init__(self, occupancy_map):
|
||||
"""
|
||||
Initialize costmap2D.
|
||||
|
||||
Args:
|
||||
----
|
||||
occupancy_map (OccupancyGrid): 2D OccupancyGrid Map
|
||||
|
||||
"""
|
||||
self.size_x = occupancy_map.info.width
|
||||
self.size_y = occupancy_map.info.height
|
||||
self.resolution = occupancy_map.info.resolution
|
||||
self.origin_x = occupancy_map.info.origin.position.x
|
||||
self.origin_y = occupancy_map.info.origin.position.y
|
||||
self.global_frame_id = occupancy_map.header.frame_id
|
||||
self.costmap_timestamp = occupancy_map.header.stamp
|
||||
# Extract costmap
|
||||
self.costmap = np.array(occupancy_map.data, dtype=np.uint8)
|
||||
|
||||
def getSizeInCellsX(self):
|
||||
"""Get map width in cells."""
|
||||
return self.size_x
|
||||
|
||||
def getSizeInCellsY(self):
|
||||
"""Get map height in cells."""
|
||||
return self.size_y
|
||||
|
||||
def getSizeInMetersX(self):
|
||||
"""Get x axis map size in meters."""
|
||||
return (self.size_x - 1 + 0.5) * self.resolution
|
||||
|
||||
def getSizeInMetersY(self):
|
||||
"""Get y axis map size in meters."""
|
||||
return (self.size_y - 1 + 0.5) * self.resolution
|
||||
|
||||
def getOriginX(self):
|
||||
"""Get the origin x axis of the map [m]."""
|
||||
return self.origin_x
|
||||
|
||||
def getOriginY(self):
|
||||
"""Get the origin y axis of the map [m]."""
|
||||
return self.origin_y
|
||||
|
||||
def getResolution(self):
|
||||
"""Get map resolution [m/cell]."""
|
||||
return self.resolution
|
||||
|
||||
def getGlobalFrameID(self):
|
||||
"""Get global frame_id."""
|
||||
return self.global_frame_id
|
||||
|
||||
def getCostmapTimestamp(self):
|
||||
"""Get costmap timestamp."""
|
||||
return self.costmap_timestamp
|
||||
|
||||
def getCostXY(self, mx: int, my: int) -> np.uint8:
|
||||
"""
|
||||
Get the cost of a cell in the costmap using map coordinate XY.
|
||||
|
||||
Args
|
||||
----
|
||||
mx (int): map coordinate X to get cost
|
||||
my (int): map coordinate Y to get cost
|
||||
|
||||
Returns
|
||||
-------
|
||||
np.uint8: cost of a cell
|
||||
|
||||
"""
|
||||
return self.costmap[self.getIndex(mx, my)]
|
||||
|
||||
def getCostIdx(self, index: int) -> np.uint8:
|
||||
"""
|
||||
Get the cost of a cell in the costmap using Index.
|
||||
|
||||
Args
|
||||
----
|
||||
index (int): index of cell to get cost
|
||||
|
||||
Returns
|
||||
-------
|
||||
np.uint8: cost of a cell
|
||||
|
||||
"""
|
||||
return self.costmap[index]
|
||||
|
||||
def setCost(self, mx: int, my: int, cost: np.uint8) -> None:
|
||||
"""
|
||||
Set the cost of a cell in the costmap using map coordinate XY.
|
||||
|
||||
Args
|
||||
----
|
||||
mx (int): map coordinate X to get cost
|
||||
my (int): map coordinate Y to get cost
|
||||
cost (np.uint8): The cost to set the cell
|
||||
|
||||
Returns
|
||||
-------
|
||||
None
|
||||
|
||||
"""
|
||||
self.costmap[self.getIndex(mx, my)] = cost
|
||||
|
||||
def mapToWorld(self, mx: int, my: int) -> tuple[float, float]:
|
||||
"""
|
||||
Get the world coordinate XY using map coordinate XY.
|
||||
|
||||
Args
|
||||
----
|
||||
mx (int): map coordinate X to get world coordinate
|
||||
my (int): map coordinate Y to get world coordinate
|
||||
|
||||
Returns
|
||||
-------
|
||||
tuple of float: wx, wy
|
||||
wx (float) [m]: world coordinate X
|
||||
wy (float) [m]: world coordinate Y
|
||||
|
||||
"""
|
||||
wx = self.origin_x + (mx + 0.5) * self.resolution
|
||||
wy = self.origin_y + (my + 0.5) * self.resolution
|
||||
return (wx, wy)
|
||||
|
||||
def worldToMap(self, wx: float, wy: float) -> tuple[int, int]:
|
||||
"""
|
||||
Get the map coordinate XY using world coordinate XY.
|
||||
|
||||
Args
|
||||
----
|
||||
wx (float) [m]: world coordinate X to get map coordinate
|
||||
wy (float) [m]: world coordinate Y to get map coordinate
|
||||
|
||||
Returns
|
||||
-------
|
||||
tuple of int: mx, my
|
||||
mx (int): map coordinate X
|
||||
my (int): map coordinate Y
|
||||
|
||||
"""
|
||||
mx = int((wx - self.origin_x) // self.resolution)
|
||||
my = int((wy - self.origin_y) // self.resolution)
|
||||
return (mx, my)
|
||||
|
||||
def getIndex(self, mx: int, my: int) -> int:
|
||||
"""
|
||||
Get the index of the cell using map coordinate XY.
|
||||
|
||||
Args
|
||||
----
|
||||
mx (int): map coordinate X to get Index
|
||||
my (int): map coordinate Y to get Index
|
||||
|
||||
Returns
|
||||
-------
|
||||
int: The index of the cell
|
||||
|
||||
"""
|
||||
return my * self.size_x + mx
|
||||
@@ -0,0 +1,100 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 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.
|
||||
|
||||
from copy import deepcopy
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
|
||||
"""
|
||||
Basic stock inspection demo. In this demonstration, the expectation
|
||||
is that there are cameras or RFID sensors mounted on the robots
|
||||
collecting information about stock quantity and location.
|
||||
"""
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Inspection route, probably read in from a file for a real application
|
||||
# from either a map or drive and repeat.
|
||||
inspection_route = [
|
||||
[3.461, -0.450],
|
||||
[5.531, -0.450],
|
||||
[3.461, -2.200],
|
||||
[5.531, -2.200],
|
||||
[3.661, -4.121],
|
||||
[5.431, -4.121],
|
||||
[3.661, -5.850],
|
||||
[5.431, -5.800]]
|
||||
|
||||
# Set our demo's initial pose
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 3.45
|
||||
initial_pose.pose.position.y = 2.15
|
||||
initial_pose.pose.orientation.z = 1.0
|
||||
initial_pose.pose.orientation.w = 0.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
# Wait for navigation to fully activate
|
||||
navigator.waitUntilNav2Active()
|
||||
|
||||
# Send our route
|
||||
inspection_points = []
|
||||
inspection_pose = PoseStamped()
|
||||
inspection_pose.header.frame_id = 'map'
|
||||
inspection_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
inspection_pose.pose.orientation.z = 1.0
|
||||
inspection_pose.pose.orientation.w = 0.0
|
||||
for pt in inspection_route:
|
||||
inspection_pose.pose.position.x = pt[0]
|
||||
inspection_pose.pose.position.y = pt[1]
|
||||
inspection_points.append(deepcopy(inspection_pose))
|
||||
navigator.followWaypoints(inspection_points)
|
||||
|
||||
# Do something during our route (e.x. AI to analyze stock information or upload to the cloud)
|
||||
# Simply the current waypoint ID for the demonstation
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
i += 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print('Executing current waypoint: ' +
|
||||
str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points)))
|
||||
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Inspection of shelves complete! Returning to start...')
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Inspection of shelving was canceled. Returning to start...')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Inspection of shelving failed! Returning to start...')
|
||||
|
||||
# go back to start
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
navigator.goToPose(initial_pose)
|
||||
while not navigator.isTaskComplete():
|
||||
pass
|
||||
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,123 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 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.
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
|
||||
# Shelf positions for picking
|
||||
shelf_positions = {
|
||||
'shelf_A': [-3.829, -7.604],
|
||||
'shelf_B': [-3.791, -3.287],
|
||||
'shelf_C': [-3.791, 1.254],
|
||||
'shelf_D': [-3.24, 5.861]}
|
||||
|
||||
# Shipping destination for picked products
|
||||
shipping_destinations = {
|
||||
'recycling': [-0.205, 7.403],
|
||||
'pallet_jack7': [-0.073, -8.497],
|
||||
'conveyer_432': [6.217, 2.153],
|
||||
'frieght_bay_3': [-6.349, 9.147]}
|
||||
|
||||
"""
|
||||
Basic item picking demo. In this demonstration, the expectation
|
||||
is that there is a person at the item shelf to put the item on the robot
|
||||
and at the pallet jack to remove it
|
||||
(probably with some kind of button for 'got item, robot go do next task').
|
||||
"""
|
||||
|
||||
|
||||
def main():
|
||||
# Recieved virtual request for picking item at Shelf A and bring to
|
||||
# worker at the pallet jack 7 for shipping. This request would
|
||||
# contain the shelf ID ("shelf_A") and shipping destination ("pallet_jack7")
|
||||
####################
|
||||
request_item_location = 'shelf_C'
|
||||
request_destination = 'pallet_jack7'
|
||||
####################
|
||||
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set our demo's initial pose
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 3.45
|
||||
initial_pose.pose.position.y = 2.15
|
||||
initial_pose.pose.orientation.z = 1.0
|
||||
initial_pose.pose.orientation.w = 0.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
# Wait for navigation to fully activate
|
||||
navigator.waitUntilNav2Active()
|
||||
|
||||
shelf_item_pose = PoseStamped()
|
||||
shelf_item_pose.header.frame_id = 'map'
|
||||
shelf_item_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
shelf_item_pose.pose.position.x = shelf_positions[request_item_location][0]
|
||||
shelf_item_pose.pose.position.y = shelf_positions[request_item_location][1]
|
||||
shelf_item_pose.pose.orientation.z = 1.0
|
||||
shelf_item_pose.pose.orientation.w = 0.0
|
||||
print(f'Received request for item picking at {request_item_location}.')
|
||||
navigator.goToPose(shelf_item_pose)
|
||||
|
||||
# Do something during our route
|
||||
# (e.x. queue up future tasks or detect person for fine-tuned positioning)
|
||||
# Simply print information for workers on the robot's ETA for the demonstation
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
i += 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print('Estimated time of arrival at ' + request_item_location +
|
||||
' for worker: ' + '{0:.0f}'.format(
|
||||
Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
|
||||
+ ' seconds.')
|
||||
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Got product from ' + request_item_location +
|
||||
'! Bringing product to shipping destination (' + request_destination + ')...')
|
||||
shipping_destination = PoseStamped()
|
||||
shipping_destination.header.frame_id = 'map'
|
||||
shipping_destination.header.stamp = navigator.get_clock().now().to_msg()
|
||||
shipping_destination.pose.position.x = shipping_destinations[request_destination][0]
|
||||
shipping_destination.pose.position.y = shipping_destinations[request_destination][1]
|
||||
shipping_destination.pose.orientation.z = 1.0
|
||||
shipping_destination.pose.orientation.w = 0.0
|
||||
navigator.goToPose(shipping_destination)
|
||||
|
||||
elif result == TaskResult.CANCELED:
|
||||
print(f'Task at {request_item_location} was canceled. Returning to staging point...')
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
navigator.goToPose(initial_pose)
|
||||
|
||||
elif result == TaskResult.FAILED:
|
||||
print(f'Task at {request_item_location} failed!')
|
||||
exit(-1)
|
||||
|
||||
while not navigator.isTaskComplete():
|
||||
pass
|
||||
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,105 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 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.
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
|
||||
"""
|
||||
Basic recoveries demo. In this demonstration, the robot navigates
|
||||
to a dead-end where recoveries such as backup and spin are used
|
||||
to get out of it.
|
||||
"""
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set our demo's initial pose
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 3.45
|
||||
initial_pose.pose.position.y = 2.15
|
||||
initial_pose.pose.orientation.z = 1.0
|
||||
initial_pose.pose.orientation.w = 0.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
# Wait for navigation to fully activate
|
||||
navigator.waitUntilNav2Active()
|
||||
|
||||
goal_pose = PoseStamped()
|
||||
goal_pose.header.frame_id = 'map'
|
||||
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose.pose.position.x = 6.13
|
||||
goal_pose.pose.position.y = 1.90
|
||||
goal_pose.pose.orientation.w = 1.0
|
||||
|
||||
navigator.goToPose(goal_pose)
|
||||
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
i += 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print(
|
||||
f'Estimated time of arrival to destination is: \
|
||||
{Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9}'
|
||||
)
|
||||
|
||||
# Robot hit a dead end, back it up
|
||||
print('Robot hit a dead end, backing up...')
|
||||
navigator.backup(backup_dist=0.5, backup_speed=0.1)
|
||||
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
i += 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print(f'Distance traveled: {feedback.distance_traveled}')
|
||||
|
||||
# Turn it around
|
||||
print('Spinning robot around...')
|
||||
navigator.spin(spin_dist=3.14)
|
||||
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
i += 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print(f'Spin angle traveled: {feedback.angular_distance_traveled}')
|
||||
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Dead end confirmed! Returning to start...')
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Recovery was canceled. Returning to start...')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Recovering from dead end failed! Returning to start...')
|
||||
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
navigator.goToPose(initial_pose)
|
||||
while not navigator.isTaskComplete():
|
||||
pass
|
||||
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,107 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 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.
|
||||
|
||||
from copy import deepcopy
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
|
||||
"""
|
||||
Basic security route patrol demo. In this demonstration, the expectation
|
||||
is that there are security cameras mounted on the robots recording or being
|
||||
watched live by security staff.
|
||||
"""
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Security route, probably read in from a file for a real application
|
||||
# from either a map or drive and repeat.
|
||||
security_route = [
|
||||
[1.792, 2.144],
|
||||
[1.792, -5.44],
|
||||
[1.792, -9.427],
|
||||
[-3.665, -9.427],
|
||||
[-3.665, -4.303],
|
||||
[-3.665, 2.330],
|
||||
[-3.665, 9.283]]
|
||||
|
||||
# Set our demo's initial pose
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 3.45
|
||||
initial_pose.pose.position.y = 2.15
|
||||
initial_pose.pose.orientation.z = 1.0
|
||||
initial_pose.pose.orientation.w = 0.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
# Wait for navigation to fully activate
|
||||
navigator.waitUntilNav2Active()
|
||||
|
||||
# Do security route until dead
|
||||
while rclpy.ok():
|
||||
# Send our route
|
||||
route_poses = []
|
||||
pose = PoseStamped()
|
||||
pose.header.frame_id = 'map'
|
||||
pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
pose.pose.orientation.w = 1.0
|
||||
for pt in security_route:
|
||||
pose.pose.position.x = pt[0]
|
||||
pose.pose.position.y = pt[1]
|
||||
route_poses.append(deepcopy(pose))
|
||||
navigator.goThroughPoses(route_poses)
|
||||
|
||||
# Do something during our route (e.x. AI detection on camera images for anomalies)
|
||||
# Simply print ETA for the demonstation
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
i += 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print('Estimated time to complete current route: ' + '{0:.0f}'.format(
|
||||
Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
|
||||
+ ' seconds.')
|
||||
|
||||
# Some failure mode, must stop since the robot is clearly stuck
|
||||
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0):
|
||||
print('Navigation has exceeded timeout of 180s, canceling request.')
|
||||
navigator.cancelTask()
|
||||
|
||||
# If at end of route, reverse the route to restart
|
||||
security_route.reverse()
|
||||
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Route complete! Restarting...')
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Security route was canceled, exiting.')
|
||||
exit(1)
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Security route failed! Restarting from other side...')
|
||||
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,56 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 2022 Joshua Wallace
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from time import sleep
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator
|
||||
import rclpy
|
||||
|
||||
"""
|
||||
Basic navigation demo to go to pose.
|
||||
"""
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set our demo's initial pose
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 3.45
|
||||
initial_pose.pose.position.y = 2.15
|
||||
initial_pose.pose.orientation.z = 1.0
|
||||
initial_pose.pose.orientation.w = 0.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
# Wait for navigation to fully activate, since autostarting nav2
|
||||
navigator.waitUntilNav2Active()
|
||||
|
||||
navigator.assistedTeleop(time_allowance=20)
|
||||
while not navigator.isTaskComplete():
|
||||
# Publish twist commands to be filtered by the assisted teleop action
|
||||
sleep(0.2)
|
||||
pass
|
||||
|
||||
navigator.lifecycleShutdown()
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,93 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 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.
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
|
||||
|
||||
"""
|
||||
Basic navigation demo to follow a given path after smoothing
|
||||
"""
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set our demo's initial pose
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 3.45
|
||||
initial_pose.pose.position.y = 2.15
|
||||
initial_pose.pose.orientation.z = 1.0
|
||||
initial_pose.pose.orientation.w = 0.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
# Wait for navigation to fully activate, since autostarting nav2
|
||||
navigator.waitUntilNav2Active()
|
||||
|
||||
# Go to our demos first goal pose
|
||||
goal_pose = PoseStamped()
|
||||
goal_pose.header.frame_id = 'map'
|
||||
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose.pose.position.x = -3.0
|
||||
goal_pose.pose.position.y = -2.0
|
||||
goal_pose.pose.orientation.w = 1.0
|
||||
|
||||
# Get the path, smooth it
|
||||
path = navigator.getPath(initial_pose, goal_pose)
|
||||
smoothed_path = navigator.smoothPath(path)
|
||||
|
||||
# Follow path
|
||||
navigator.followPath(smoothed_path)
|
||||
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
################################################
|
||||
#
|
||||
# Implement some code here for your application!
|
||||
#
|
||||
################################################
|
||||
|
||||
# Do something with the feedback
|
||||
i += 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print('Estimated distance remaining to goal position: ' +
|
||||
'{0:.3f}'.format(feedback.distance_to_goal) +
|
||||
'\nCurrent speed of the robot: ' +
|
||||
'{0:.3f}'.format(feedback.speed))
|
||||
|
||||
# Do something depending on the return code
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Goal succeeded!')
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Goal was canceled!')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Goal failed!')
|
||||
else:
|
||||
print('Goal has an invalid return status!')
|
||||
|
||||
navigator.lifecycleShutdown()
|
||||
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,139 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 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.
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
"""
|
||||
Basic navigation demo to go to poses.
|
||||
"""
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set our demo's initial pose
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 3.45
|
||||
initial_pose.pose.position.y = 2.15
|
||||
initial_pose.pose.orientation.z = 1.0
|
||||
initial_pose.pose.orientation.w = 0.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
# Activate navigation, if not autostarted. This should be called after setInitialPose()
|
||||
# or this will initialize at the origin of the map and update the costmap with bogus readings.
|
||||
# If autostart, you should `waitUntilNav2Active()` instead.
|
||||
# navigator.lifecycleStartup()
|
||||
|
||||
# Wait for navigation to fully activate, since autostarting nav2
|
||||
navigator.waitUntilNav2Active()
|
||||
|
||||
# If desired, you can change or load the map as well
|
||||
# navigator.changeMap('/path/to/map.yaml')
|
||||
|
||||
# You may use the navigator to clear or obtain costmaps
|
||||
# navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap()
|
||||
# global_costmap = navigator.getGlobalCostmap()
|
||||
# local_costmap = navigator.getLocalCostmap()
|
||||
|
||||
# set our demo's goal poses
|
||||
goal_poses = []
|
||||
goal_pose1 = PoseStamped()
|
||||
goal_pose1.header.frame_id = 'map'
|
||||
goal_pose1.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose1.pose.position.x = 1.5
|
||||
goal_pose1.pose.position.y = 0.55
|
||||
goal_pose1.pose.orientation.w = 0.707
|
||||
goal_pose1.pose.orientation.z = 0.707
|
||||
goal_poses.append(goal_pose1)
|
||||
|
||||
# additional goals can be appended
|
||||
goal_pose2 = PoseStamped()
|
||||
goal_pose2.header.frame_id = 'map'
|
||||
goal_pose2.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose2.pose.position.x = 1.5
|
||||
goal_pose2.pose.position.y = -3.75
|
||||
goal_pose2.pose.orientation.w = 0.707
|
||||
goal_pose2.pose.orientation.z = 0.707
|
||||
goal_poses.append(goal_pose2)
|
||||
goal_pose3 = PoseStamped()
|
||||
goal_pose3.header.frame_id = 'map'
|
||||
goal_pose3.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose3.pose.position.x = -3.6
|
||||
goal_pose3.pose.position.y = -4.75
|
||||
goal_pose3.pose.orientation.w = 0.707
|
||||
goal_pose3.pose.orientation.z = 0.707
|
||||
goal_poses.append(goal_pose3)
|
||||
|
||||
# sanity check a valid path exists
|
||||
# path = navigator.getPathThroughPoses(initial_pose, goal_poses)
|
||||
|
||||
navigator.goThroughPoses(goal_poses)
|
||||
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
################################################
|
||||
#
|
||||
# Implement some code here for your application!
|
||||
#
|
||||
################################################
|
||||
|
||||
# Do something with the feedback
|
||||
i = i + 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print('Estimated time of arrival: ' + '{0:.0f}'.format(
|
||||
Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
|
||||
+ ' seconds.')
|
||||
|
||||
# Some navigation timeout to demo cancellation
|
||||
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
|
||||
navigator.cancelTask()
|
||||
|
||||
# Some navigation request change to demo preemption
|
||||
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=35.0):
|
||||
goal_pose4 = PoseStamped()
|
||||
goal_pose4.header.frame_id = 'map'
|
||||
goal_pose4.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose4.pose.position.x = -5.0
|
||||
goal_pose4.pose.position.y = -4.75
|
||||
goal_pose4.pose.orientation.w = 0.707
|
||||
goal_pose4.pose.orientation.z = 0.707
|
||||
navigator.goThroughPoses([goal_pose4])
|
||||
|
||||
# Do something depending on the return code
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Goal succeeded!')
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Goal was canceled!')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Goal failed!')
|
||||
else:
|
||||
print('Goal has an invalid return status!')
|
||||
|
||||
navigator.lifecycleShutdown()
|
||||
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,112 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 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.
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
"""
|
||||
Basic navigation demo to go to pose.
|
||||
"""
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set our demo's initial pose
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 3.45
|
||||
initial_pose.pose.position.y = 2.15
|
||||
initial_pose.pose.orientation.z = 1.0
|
||||
initial_pose.pose.orientation.w = 0.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
# Activate navigation, if not autostarted. This should be called after setInitialPose()
|
||||
# or this will initialize at the origin of the map and update the costmap with bogus readings.
|
||||
# If autostart, you should `waitUntilNav2Active()` instead.
|
||||
# navigator.lifecycleStartup()
|
||||
|
||||
# Wait for navigation to fully activate, since autostarting nav2
|
||||
navigator.waitUntilNav2Active()
|
||||
|
||||
# If desired, you can change or load the map as well
|
||||
# navigator.changeMap('/path/to/map.yaml')
|
||||
|
||||
# You may use the navigator to clear or obtain costmaps
|
||||
# navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap()
|
||||
# global_costmap = navigator.getGlobalCostmap()
|
||||
# local_costmap = navigator.getLocalCostmap()
|
||||
|
||||
# Go to our demos first goal pose
|
||||
goal_pose = PoseStamped()
|
||||
goal_pose.header.frame_id = 'map'
|
||||
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose.pose.position.x = -2.0
|
||||
goal_pose.pose.position.y = -0.5
|
||||
goal_pose.pose.orientation.w = 1.0
|
||||
|
||||
# sanity check a valid path exists
|
||||
# path = navigator.getPath(initial_pose, goal_pose)
|
||||
|
||||
navigator.goToPose(goal_pose)
|
||||
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
################################################
|
||||
#
|
||||
# Implement some code here for your application!
|
||||
#
|
||||
################################################
|
||||
|
||||
# Do something with the feedback
|
||||
i = i + 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print('Estimated time of arrival: ' + '{0:.0f}'.format(
|
||||
Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
|
||||
+ ' seconds.')
|
||||
|
||||
# Some navigation timeout to demo cancellation
|
||||
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
|
||||
navigator.cancelTask()
|
||||
|
||||
# Some navigation request change to demo preemption
|
||||
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0):
|
||||
goal_pose.pose.position.x = -3.0
|
||||
navigator.goToPose(goal_pose)
|
||||
|
||||
# Do something depending on the return code
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Goal succeeded!')
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Goal was canceled!')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Goal failed!')
|
||||
else:
|
||||
print('Goal has an invalid return status!')
|
||||
|
||||
navigator.lifecycleShutdown()
|
||||
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,142 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 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.
|
||||
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
|
||||
import rclpy
|
||||
from rclpy.duration import Duration
|
||||
|
||||
"""
|
||||
Basic navigation demo to go to poses.
|
||||
"""
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
navigator = BasicNavigator()
|
||||
|
||||
# Set our demo's initial pose
|
||||
initial_pose = PoseStamped()
|
||||
initial_pose.header.frame_id = 'map'
|
||||
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
|
||||
initial_pose.pose.position.x = 3.45
|
||||
initial_pose.pose.position.y = 2.15
|
||||
initial_pose.pose.orientation.z = 1.0
|
||||
initial_pose.pose.orientation.w = 0.0
|
||||
navigator.setInitialPose(initial_pose)
|
||||
|
||||
# Activate navigation, if not autostarted. This should be called after setInitialPose()
|
||||
# or this will initialize at the origin of the map and update the costmap with bogus readings.
|
||||
# If autostart, you should `waitUntilNav2Active()` instead.
|
||||
# navigator.lifecycleStartup()
|
||||
|
||||
# Wait for navigation to fully activate, since autostarting nav2
|
||||
navigator.waitUntilNav2Active()
|
||||
|
||||
# If desired, you can change or load the map as well
|
||||
# navigator.changeMap('/path/to/map.yaml')
|
||||
|
||||
# You may use the navigator to clear or obtain costmaps
|
||||
# navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap()
|
||||
# global_costmap = navigator.getGlobalCostmap()
|
||||
# local_costmap = navigator.getLocalCostmap()
|
||||
|
||||
# set our demo's goal poses to follow
|
||||
goal_poses = []
|
||||
goal_pose1 = PoseStamped()
|
||||
goal_pose1.header.frame_id = 'map'
|
||||
goal_pose1.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose1.pose.position.x = 1.5
|
||||
goal_pose1.pose.position.y = 0.55
|
||||
goal_pose1.pose.orientation.w = 0.707
|
||||
goal_pose1.pose.orientation.z = 0.707
|
||||
goal_poses.append(goal_pose1)
|
||||
|
||||
# additional goals can be appended
|
||||
goal_pose2 = PoseStamped()
|
||||
goal_pose2.header.frame_id = 'map'
|
||||
goal_pose2.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose2.pose.position.x = 1.5
|
||||
goal_pose2.pose.position.y = -3.75
|
||||
goal_pose2.pose.orientation.w = 0.707
|
||||
goal_pose2.pose.orientation.z = 0.707
|
||||
goal_poses.append(goal_pose2)
|
||||
goal_pose3 = PoseStamped()
|
||||
goal_pose3.header.frame_id = 'map'
|
||||
goal_pose3.header.stamp = navigator.get_clock().now().to_msg()
|
||||
goal_pose3.pose.position.x = -3.6
|
||||
goal_pose3.pose.position.y = -4.75
|
||||
goal_pose3.pose.orientation.w = 0.707
|
||||
goal_pose3.pose.orientation.z = 0.707
|
||||
goal_poses.append(goal_pose3)
|
||||
|
||||
# sanity check a valid path exists
|
||||
# path = navigator.getPath(initial_pose, goal_pose1)
|
||||
|
||||
nav_start = navigator.get_clock().now()
|
||||
navigator.followWaypoints(goal_poses)
|
||||
|
||||
i = 0
|
||||
while not navigator.isTaskComplete():
|
||||
################################################
|
||||
#
|
||||
# Implement some code here for your application!
|
||||
#
|
||||
################################################
|
||||
|
||||
# Do something with the feedback
|
||||
i = i + 1
|
||||
feedback = navigator.getFeedback()
|
||||
if feedback and i % 5 == 0:
|
||||
print('Executing current waypoint: ' +
|
||||
str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses)))
|
||||
now = navigator.get_clock().now()
|
||||
|
||||
# Some navigation timeout to demo cancellation
|
||||
if now - nav_start > Duration(seconds=600.0):
|
||||
navigator.cancelTask()
|
||||
|
||||
# Some follow waypoints request change to demo preemption
|
||||
if now - nav_start > Duration(seconds=35.0):
|
||||
goal_pose4 = PoseStamped()
|
||||
goal_pose4.header.frame_id = 'map'
|
||||
goal_pose4.header.stamp = now.to_msg()
|
||||
goal_pose4.pose.position.x = -5.0
|
||||
goal_pose4.pose.position.y = -4.75
|
||||
goal_pose4.pose.orientation.w = 0.707
|
||||
goal_pose4.pose.orientation.z = 0.707
|
||||
goal_poses = [goal_pose4]
|
||||
nav_start = now
|
||||
navigator.followWaypoints(goal_poses)
|
||||
|
||||
# Do something depending on the return code
|
||||
result = navigator.getResult()
|
||||
if result == TaskResult.SUCCEEDED:
|
||||
print('Goal succeeded!')
|
||||
elif result == TaskResult.CANCELED:
|
||||
print('Goal was canceled!')
|
||||
elif result == TaskResult.FAILED:
|
||||
print('Goal failed!')
|
||||
else:
|
||||
print('Goal has an invalid return status!')
|
||||
|
||||
navigator.lifecycleShutdown()
|
||||
|
||||
exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
+220
@@ -0,0 +1,220 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 2021 Samsung Research America
|
||||
# Copyright 2022 Afif Swaidan
|
||||
#
|
||||
# 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 a Python3 API for a Footprint Collision Checker.
|
||||
|
||||
It provides the needed methods to manipulate the coordinates
|
||||
and calculate the cost of a Footprint
|
||||
"""
|
||||
|
||||
from math import cos, sin
|
||||
|
||||
from geometry_msgs.msg import Point32, Polygon
|
||||
from nav2_simple_commander.costmap_2d import PyCostmap2D
|
||||
from nav2_simple_commander.line_iterator import LineIterator
|
||||
|
||||
NO_INFORMATION = 255
|
||||
LETHAL_OBSTACLE = 254
|
||||
INSCRIBED_INFLATED_OBSTACLE = 253
|
||||
MAX_NON_OBSTACLE = 252
|
||||
FREE_SPACE = 0
|
||||
|
||||
|
||||
class FootprintCollisionChecker:
|
||||
"""
|
||||
FootprintCollisionChecker.
|
||||
|
||||
FootprintCollisionChecker Class for getting the cost
|
||||
and checking the collisions of a Footprint
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
"""Initialize the FootprintCollisionChecker Object."""
|
||||
self.costmap_ = None
|
||||
pass
|
||||
|
||||
def footprintCost(self, footprint: Polygon):
|
||||
"""
|
||||
Iterate over all the points in a footprint and check for collision.
|
||||
|
||||
Args
|
||||
----
|
||||
footprint (Polygon): The footprint to calculate the collision cost for
|
||||
|
||||
Returns
|
||||
-------
|
||||
LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
|
||||
footprint_cost (float): The maximum cost found in the footprint points
|
||||
|
||||
"""
|
||||
footprint_cost = 0.0
|
||||
x1 = 0.0
|
||||
y1 = 0.0
|
||||
|
||||
x0, y0 = self.worldToMapValidated(footprint.points[0].x, footprint.points[0].y)
|
||||
|
||||
if x0 is None or y0 is None:
|
||||
return LETHAL_OBSTACLE
|
||||
|
||||
xstart = x0
|
||||
ystart = y0
|
||||
|
||||
for i in range(len(footprint.points) - 1):
|
||||
x1, y1 = self.worldToMapValidated(
|
||||
footprint.points[i + 1].x, footprint.points[i + 1].y
|
||||
)
|
||||
|
||||
if x1 is None or y1 is None:
|
||||
return LETHAL_OBSTACLE
|
||||
|
||||
footprint_cost = max(float(self.lineCost(x0, x1, y0, y1)), footprint_cost)
|
||||
x0 = x1
|
||||
y0 = y1
|
||||
|
||||
if footprint_cost == LETHAL_OBSTACLE:
|
||||
return footprint_cost
|
||||
|
||||
return max(float(self.lineCost(xstart, x1, ystart, y1)), footprint_cost)
|
||||
|
||||
def lineCost(self, x0, x1, y0, y1, step_size=0.5):
|
||||
"""
|
||||
Iterate over all the points along a line and check for collision.
|
||||
|
||||
Args
|
||||
----
|
||||
x0 (float): Abscissa of the initial point in map coordinates
|
||||
y0 (float): Ordinate of the initial point in map coordinates
|
||||
x1 (float): Abscissa of the final point in map coordinates
|
||||
y1 (float): Ordinate of the final point in map coordinates
|
||||
step_size (float): Optional, Increments' resolution, defaults to 0.5
|
||||
|
||||
Returns
|
||||
-------
|
||||
LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
|
||||
line_cost (float): The maximum cost found in the line points
|
||||
|
||||
"""
|
||||
line_cost = 0.0
|
||||
point_cost = -1.0
|
||||
line_iterator = LineIterator(x0, y0, x1, y1, step_size)
|
||||
|
||||
while line_iterator.isValid():
|
||||
point_cost = self.pointCost(
|
||||
int(line_iterator.getX()), int(line_iterator.getY())
|
||||
)
|
||||
|
||||
if point_cost == LETHAL_OBSTACLE:
|
||||
return point_cost
|
||||
|
||||
if line_cost < point_cost:
|
||||
line_cost = point_cost
|
||||
|
||||
line_iterator.advance()
|
||||
|
||||
return line_cost
|
||||
|
||||
def worldToMapValidated(self, wx: float, wy: float):
|
||||
"""
|
||||
Get the map coordinate XY using world coordinate XY.
|
||||
|
||||
Args
|
||||
----
|
||||
wx (float): world coordinate X
|
||||
wy (float): world coordinate Y
|
||||
|
||||
Returns
|
||||
-------
|
||||
None: if coordinates are invalid
|
||||
tuple of int: mx, my (if coordinates are valid)
|
||||
mx (int): map coordinate X
|
||||
my (int): map coordinate Y
|
||||
|
||||
"""
|
||||
if self.costmap_ is None:
|
||||
raise ValueError(
|
||||
'Costmap not specified, use setCostmap to specify the costmap first'
|
||||
)
|
||||
return self.costmap_.worldToMap(wx, wy)
|
||||
|
||||
def pointCost(self, x: int, y: int):
|
||||
"""
|
||||
Get the cost of a point in the costmap using map coordinates XY.
|
||||
|
||||
Args
|
||||
----
|
||||
mx (int): map coordinate X
|
||||
my (int): map coordinate Y
|
||||
|
||||
Returns
|
||||
-------
|
||||
np.uint8: cost of a point
|
||||
|
||||
"""
|
||||
if self.costmap_ is None:
|
||||
raise ValueError(
|
||||
'Costmap not specified, use setCostmap to specify the costmap first'
|
||||
)
|
||||
return self.costmap_.getCostXY(x, y)
|
||||
|
||||
def setCostmap(self, costmap: PyCostmap2D):
|
||||
"""
|
||||
Specify which costmap to use.
|
||||
|
||||
Args
|
||||
----
|
||||
costmap (PyCostmap2D): costmap to use in the object's methods
|
||||
|
||||
Returns
|
||||
-------
|
||||
None
|
||||
|
||||
"""
|
||||
self.costmap_ = costmap
|
||||
return None
|
||||
|
||||
def footprintCostAtPose(self, x: float, y: float, theta: float, footprint: Polygon):
|
||||
"""
|
||||
Get the cost of a footprint at a specific Pose in map coordinates.
|
||||
|
||||
Args
|
||||
----
|
||||
x (float): map coordinate X
|
||||
y (float): map coordinate Y
|
||||
theta (float): absolute rotation angle of the footprint
|
||||
footprint (Polygon): the footprint to calculate its cost at the given Pose
|
||||
|
||||
Returns
|
||||
-------
|
||||
LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
|
||||
footprint_cost (float): The maximum cost found in the footprint points
|
||||
|
||||
"""
|
||||
cos_th = cos(theta)
|
||||
sin_th = sin(theta)
|
||||
oriented_footprint = Polygon()
|
||||
|
||||
for i in range(len(footprint.points)):
|
||||
new_pt = Point32()
|
||||
new_pt.x = x + (
|
||||
footprint.points[i].x * cos_th - footprint.points[i].y * sin_th
|
||||
)
|
||||
new_pt.y = y + (
|
||||
footprint.points[i].x * sin_th + footprint.points[i].y * cos_th
|
||||
)
|
||||
oriented_footprint.points.append(new_pt)
|
||||
|
||||
return self.footprintCost(oriented_footprint)
|
||||
@@ -0,0 +1,177 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 2021 Samsung Research America
|
||||
# Copyright 2022 Afif Swaidan
|
||||
#
|
||||
# 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 a Python3 API for a line iterator.
|
||||
|
||||
It provides the ability to iterate
|
||||
through the points of a line.
|
||||
"""
|
||||
|
||||
from cmath import sqrt
|
||||
|
||||
|
||||
class LineIterator():
|
||||
"""
|
||||
LineIterator.
|
||||
|
||||
LineIterator Python3 API for iterating along the points of a given line
|
||||
"""
|
||||
|
||||
def __init__(self, x0, y0, x1, y1, step_size=1.0):
|
||||
"""
|
||||
Initialize the LineIterator.
|
||||
|
||||
Args
|
||||
----
|
||||
x0 (float): Abscissa of the initial point
|
||||
y0 (float): Ordinate of the initial point
|
||||
x1 (float): Abscissa of the final point
|
||||
y1 (float): Ordinate of the final point
|
||||
step_size (float): Optional, Increments' resolution, defaults to 1
|
||||
|
||||
Raises
|
||||
------
|
||||
TypeError: When one (or more) of the inputs is not a number
|
||||
ValueError: When step_size is not a positive number
|
||||
|
||||
"""
|
||||
if type(x0) not in [int, float]:
|
||||
raise TypeError("x0 must be a number (int or float)")
|
||||
|
||||
if type(y0) not in [int, float]:
|
||||
raise TypeError("y0 must be a number (int or float)")
|
||||
|
||||
if type(x1) not in [int, float]:
|
||||
raise TypeError("x1 must be a number (int or float)")
|
||||
|
||||
if type(y1) not in [int, float]:
|
||||
raise TypeError("y1 must be a number (int or float)")
|
||||
|
||||
if type(step_size) not in [int, float]:
|
||||
raise TypeError("step_size must be a number (int or float)")
|
||||
|
||||
if step_size <= 0:
|
||||
raise ValueError("step_size must be a positive number")
|
||||
|
||||
self.x0_ = x0
|
||||
self.y0_ = y0
|
||||
self.x1_ = x1
|
||||
self.y1_ = y1
|
||||
self.x_ = x0
|
||||
self.y_ = y0
|
||||
self.step_size_ = step_size
|
||||
|
||||
if x1 != x0 and y1 != y0:
|
||||
self.valid_ = True
|
||||
self.m_ = (y1-y0)/(x1-x0)
|
||||
self.b_ = y1 - (self.m_*x1)
|
||||
elif x1 == x0 and y1 != y0:
|
||||
self.valid_ = True
|
||||
elif y1 == y1 and x1 != x0:
|
||||
self.valid_ = True
|
||||
self.m_ = (y1-y0)/(x1-x0)
|
||||
self.b_ = y1 - (self.m_*x1)
|
||||
else:
|
||||
self.valid_ = False
|
||||
raise ValueError(
|
||||
"Line has zero length (All 4 points have same coordinates)")
|
||||
|
||||
def isValid(self):
|
||||
"""Check if line is valid."""
|
||||
return self.valid_
|
||||
|
||||
def advance(self):
|
||||
"""Advance to the next point in the line."""
|
||||
if self.x1_ > self.x0_:
|
||||
if self.x_ < self.x1_:
|
||||
self.x_ = round(self.clamp(
|
||||
self.x_ + self.step_size_, self.x0_, self.x1_), 5)
|
||||
self.y_ = round(self.m_ * self.x_ + self.b_, 5)
|
||||
else:
|
||||
self.valid_ = False
|
||||
elif self.x1_ < self.x0_:
|
||||
if self.x_ > self.x1_:
|
||||
self.x_ = round(self.clamp(
|
||||
self.x_ - self.step_size_, self.x1_, self.x0_), 5)
|
||||
self.y_ = round(self.m_ * self.x_ + self.b_, 5)
|
||||
else:
|
||||
self.valid_ = False
|
||||
else:
|
||||
if self.y1_ > self.y0_:
|
||||
if self.y_ < self.y1_:
|
||||
self.y_ = round(self.clamp(
|
||||
self.y_ + self.step_size_, self.y0_, self.y1_), 5)
|
||||
else:
|
||||
self.valid_ = False
|
||||
elif self.y1_ < self.y0_:
|
||||
if self.y_ > self.y1_:
|
||||
self.y_ = round(self.clamp(
|
||||
self.y_ - self.step_size_, self.y1_, self.y0_), 5)
|
||||
else:
|
||||
self.valid_ = False
|
||||
else:
|
||||
self.valid_ = False
|
||||
|
||||
def getX(self):
|
||||
"""Get the abscissa of the current point."""
|
||||
return self.x_
|
||||
|
||||
def getY(self):
|
||||
"""Get the ordinate of the current point."""
|
||||
return self.y_
|
||||
|
||||
def getX0(self):
|
||||
"""Get the abscissa of the initial point."""
|
||||
return self.x0_
|
||||
|
||||
def getY0(self):
|
||||
"""Get the ordinate of the intial point."""
|
||||
return self.y0_
|
||||
|
||||
def getX1(self):
|
||||
"""Get the abscissa of the final point."""
|
||||
return self.x1_
|
||||
|
||||
def getY1(self):
|
||||
"""Get the ordinate of the final point."""
|
||||
return self.y1_
|
||||
|
||||
def get_line_length(self):
|
||||
"""Get the length of the line."""
|
||||
return sqrt(pow(self.x1_ - self.x0_, 2) + pow(self.y1_ - self.y0_, 2))
|
||||
|
||||
def clamp(self, n, min_n, max_n):
|
||||
"""
|
||||
Clamp n to be between min_n and max_n.
|
||||
|
||||
Args
|
||||
----
|
||||
n (float): input value
|
||||
min_n (float): minimum value
|
||||
max_n (float): maximum value
|
||||
|
||||
Returns
|
||||
-------
|
||||
n (float): input value clamped between given min and max
|
||||
|
||||
"""
|
||||
if n < min_n:
|
||||
return min_n
|
||||
elif n > max_n:
|
||||
return max_n
|
||||
else:
|
||||
return n
|
||||
@@ -0,0 +1,593 @@
|
||||
#! /usr/bin/env python3
|
||||
# Copyright 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.
|
||||
|
||||
|
||||
from enum import Enum
|
||||
import time
|
||||
|
||||
from action_msgs.msg import GoalStatus
|
||||
from builtin_interfaces.msg import Duration
|
||||
from geometry_msgs.msg import Point
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from geometry_msgs.msg import PoseWithCovarianceStamped
|
||||
from lifecycle_msgs.srv import GetState
|
||||
from nav2_msgs.action import AssistedTeleop, BackUp, Spin
|
||||
from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose
|
||||
from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose
|
||||
from nav2_msgs.action import SmoothPath
|
||||
from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.duration import Duration as rclpyDuration
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy
|
||||
from rclpy.qos import QoSProfile, QoSReliabilityPolicy
|
||||
|
||||
|
||||
class TaskResult(Enum):
|
||||
UNKNOWN = 0
|
||||
SUCCEEDED = 1
|
||||
CANCELED = 2
|
||||
FAILED = 3
|
||||
|
||||
|
||||
class BasicNavigator(Node):
|
||||
|
||||
def __init__(self, node_name='basic_navigator', namespace=''):
|
||||
super().__init__(node_name=node_name, namespace=namespace)
|
||||
self.initial_pose = PoseStamped()
|
||||
self.initial_pose.header.frame_id = 'map'
|
||||
self.goal_handle = None
|
||||
self.result_future = None
|
||||
self.feedback = None
|
||||
self.status = None
|
||||
|
||||
amcl_pose_qos = QoSProfile(
|
||||
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
|
||||
reliability=QoSReliabilityPolicy.RELIABLE,
|
||||
history=QoSHistoryPolicy.KEEP_LAST,
|
||||
depth=1)
|
||||
|
||||
self.initial_pose_received = False
|
||||
self.nav_through_poses_client = ActionClient(self,
|
||||
NavigateThroughPoses,
|
||||
'navigate_through_poses')
|
||||
self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
|
||||
self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints')
|
||||
self.follow_path_client = ActionClient(self, FollowPath, 'follow_path')
|
||||
self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose,
|
||||
'compute_path_to_pose')
|
||||
self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses,
|
||||
'compute_path_through_poses')
|
||||
self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path')
|
||||
self.spin_client = ActionClient(self, Spin, 'spin')
|
||||
self.backup_client = ActionClient(self, BackUp, 'backup')
|
||||
self.assisted_teleop_client = ActionClient(self, AssistedTeleop, 'assisted_teleop')
|
||||
self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
|
||||
'amcl_pose',
|
||||
self._amclPoseCallback,
|
||||
amcl_pose_qos)
|
||||
self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
|
||||
'initialpose',
|
||||
10)
|
||||
self.change_maps_srv = self.create_client(LoadMap, 'map_server/load_map')
|
||||
self.clear_costmap_global_srv = self.create_client(
|
||||
ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap')
|
||||
self.clear_costmap_local_srv = self.create_client(
|
||||
ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap')
|
||||
self.get_costmap_global_srv = self.create_client(GetCostmap, 'global_costmap/get_costmap')
|
||||
self.get_costmap_local_srv = self.create_client(GetCostmap, 'local_costmap/get_costmap')
|
||||
|
||||
def destroyNode(self):
|
||||
self.destroy_node()
|
||||
|
||||
def destroy_node(self):
|
||||
self.nav_through_poses_client.destroy()
|
||||
self.nav_to_pose_client.destroy()
|
||||
self.follow_waypoints_client.destroy()
|
||||
self.follow_path_client.destroy()
|
||||
self.compute_path_to_pose_client.destroy()
|
||||
self.compute_path_through_poses_client.destroy()
|
||||
self.smoother_client.destroy()
|
||||
self.spin_client.destroy()
|
||||
self.backup_client.destroy()
|
||||
super().destroy_node()
|
||||
|
||||
def setInitialPose(self, initial_pose):
|
||||
"""Set the initial pose to the localization system."""
|
||||
self.initial_pose_received = False
|
||||
self.initial_pose = initial_pose
|
||||
self._setInitialPose()
|
||||
|
||||
def goThroughPoses(self, poses, behavior_tree=''):
|
||||
"""Send a `NavThroughPoses` action request."""
|
||||
self.debug("Waiting for 'NavigateThroughPoses' action server")
|
||||
while not self.nav_through_poses_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'NavigateThroughPoses' action server not available, waiting...")
|
||||
|
||||
goal_msg = NavigateThroughPoses.Goal()
|
||||
goal_msg.poses = poses
|
||||
goal_msg.behavior_tree = behavior_tree
|
||||
|
||||
self.info(f'Navigating with {len(goal_msg.poses)} goals....')
|
||||
send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg,
|
||||
self._feedbackCallback)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error(f'Goal with {len(poses)} poses was rejected!')
|
||||
return False
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
return True
|
||||
|
||||
def goToPose(self, pose, behavior_tree=''):
|
||||
"""Send a `NavToPose` action request."""
|
||||
self.debug("Waiting for 'NavigateToPose' action server")
|
||||
while not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'NavigateToPose' action server not available, waiting...")
|
||||
|
||||
goal_msg = NavigateToPose.Goal()
|
||||
goal_msg.pose = pose
|
||||
goal_msg.behavior_tree = behavior_tree
|
||||
|
||||
self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' +
|
||||
str(pose.pose.position.y) + '...')
|
||||
send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg,
|
||||
self._feedbackCallback)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error('Goal to ' + str(pose.pose.position.x) + ' ' +
|
||||
str(pose.pose.position.y) + ' was rejected!')
|
||||
return False
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
return True
|
||||
|
||||
def followWaypoints(self, poses):
|
||||
"""Send a `FollowWaypoints` action request."""
|
||||
self.debug("Waiting for 'FollowWaypoints' action server")
|
||||
while not self.follow_waypoints_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'FollowWaypoints' action server not available, waiting...")
|
||||
|
||||
goal_msg = FollowWaypoints.Goal()
|
||||
goal_msg.poses = poses
|
||||
|
||||
self.info(f'Following {len(goal_msg.poses)} goals....')
|
||||
send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg,
|
||||
self._feedbackCallback)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error(f'Following {len(poses)} waypoints request was rejected!')
|
||||
return False
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
return True
|
||||
|
||||
def spin(self, spin_dist=1.57, time_allowance=10):
|
||||
self.debug("Waiting for 'Spin' action server")
|
||||
while not self.spin_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'Spin' action server not available, waiting...")
|
||||
goal_msg = Spin.Goal()
|
||||
goal_msg.target_yaw = spin_dist
|
||||
goal_msg.time_allowance = Duration(sec=time_allowance)
|
||||
|
||||
self.info(f'Spinning to angle {goal_msg.target_yaw}....')
|
||||
send_goal_future = self.spin_client.send_goal_async(goal_msg, self._feedbackCallback)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error('Spin request was rejected!')
|
||||
return False
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
return True
|
||||
|
||||
def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10):
|
||||
self.debug("Waiting for 'Backup' action server")
|
||||
while not self.backup_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'Backup' action server not available, waiting...")
|
||||
goal_msg = BackUp.Goal()
|
||||
goal_msg.target = Point(x=float(backup_dist))
|
||||
goal_msg.speed = backup_speed
|
||||
goal_msg.time_allowance = Duration(sec=time_allowance)
|
||||
|
||||
self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....')
|
||||
send_goal_future = self.backup_client.send_goal_async(goal_msg, self._feedbackCallback)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error('Backup request was rejected!')
|
||||
return False
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
return True
|
||||
|
||||
def assistedTeleop(self, time_allowance=30):
|
||||
self.debug("Wainting for 'assisted_teleop' action server")
|
||||
while not self.assisted_teleop_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'assisted_teleop' action server not available, waiting...")
|
||||
goal_msg = AssistedTeleop.Goal()
|
||||
goal_msg.time_allowance = Duration(sec=time_allowance)
|
||||
|
||||
self.info("Running 'assisted_teleop'....")
|
||||
send_goal_future = \
|
||||
self.assisted_teleop_client.send_goal_async(goal_msg, self._feedbackCallback)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error('Assisted Teleop request was rejected!')
|
||||
return False
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
return True
|
||||
|
||||
def followPath(self, path, controller_id='', goal_checker_id=''):
|
||||
"""Send a `FollowPath` action request."""
|
||||
self.debug("Waiting for 'FollowPath' action server")
|
||||
while not self.follow_path_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'FollowPath' action server not available, waiting...")
|
||||
|
||||
goal_msg = FollowPath.Goal()
|
||||
goal_msg.path = path
|
||||
goal_msg.controller_id = controller_id
|
||||
goal_msg.goal_checker_id = goal_checker_id
|
||||
|
||||
self.info('Executing path...')
|
||||
send_goal_future = self.follow_path_client.send_goal_async(goal_msg,
|
||||
self._feedbackCallback)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error('Follow path was rejected!')
|
||||
return False
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
return True
|
||||
|
||||
def cancelTask(self):
|
||||
"""Cancel pending task request of any type."""
|
||||
self.info('Canceling current task.')
|
||||
if self.result_future:
|
||||
future = self.goal_handle.cancel_goal_async()
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
return
|
||||
|
||||
def isTaskComplete(self):
|
||||
"""Check if the task request of any type is complete yet."""
|
||||
if not self.result_future:
|
||||
# task was cancelled or completed
|
||||
return True
|
||||
rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10)
|
||||
if self.result_future.result():
|
||||
self.status = self.result_future.result().status
|
||||
if self.status != GoalStatus.STATUS_SUCCEEDED:
|
||||
self.debug(f'Task with failed with status code: {self.status}')
|
||||
return True
|
||||
else:
|
||||
# Timed out, still processing, not complete yet
|
||||
return False
|
||||
|
||||
self.debug('Task succeeded!')
|
||||
return True
|
||||
|
||||
def getFeedback(self):
|
||||
"""Get the pending action feedback message."""
|
||||
return self.feedback
|
||||
|
||||
def getResult(self):
|
||||
"""Get the pending action result message."""
|
||||
if self.status == GoalStatus.STATUS_SUCCEEDED:
|
||||
return TaskResult.SUCCEEDED
|
||||
elif self.status == GoalStatus.STATUS_ABORTED:
|
||||
return TaskResult.FAILED
|
||||
elif self.status == GoalStatus.STATUS_CANCELED:
|
||||
return TaskResult.CANCELED
|
||||
else:
|
||||
return TaskResult.UNKNOWN
|
||||
|
||||
def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
|
||||
"""Block until the full navigation system is up and running."""
|
||||
self._waitForNodeToActivate(localizer)
|
||||
if localizer == 'amcl':
|
||||
self._waitForInitialPose()
|
||||
self._waitForNodeToActivate(navigator)
|
||||
self.info('Nav2 is ready for use!')
|
||||
return
|
||||
|
||||
def _getPathImpl(self, start, goal, planner_id='', use_start=False):
|
||||
"""
|
||||
Send a `ComputePathToPose` action request.
|
||||
|
||||
Internal implementation to get the full result, not just the path.
|
||||
"""
|
||||
self.debug("Waiting for 'ComputePathToPose' action server")
|
||||
while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'ComputePathToPose' action server not available, waiting...")
|
||||
|
||||
goal_msg = ComputePathToPose.Goal()
|
||||
goal_msg.start = start
|
||||
goal_msg.goal = goal
|
||||
goal_msg.planner_id = planner_id
|
||||
goal_msg.use_start = use_start
|
||||
|
||||
self.info('Getting path...')
|
||||
send_goal_future = self.compute_path_to_pose_client.send_goal_async(goal_msg)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error('Get path was rejected!')
|
||||
return None
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
rclpy.spin_until_future_complete(self, self.result_future)
|
||||
self.status = self.result_future.result().status
|
||||
if self.status != GoalStatus.STATUS_SUCCEEDED:
|
||||
self.warn(f'Getting path failed with status code: {self.status}')
|
||||
return None
|
||||
|
||||
return self.result_future.result().result
|
||||
|
||||
def getPath(self, start, goal, planner_id='', use_start=False):
|
||||
"""Send a `ComputePathToPose` action request."""
|
||||
rtn = self._getPathImpl(start, goal, planner_id, use_start)
|
||||
if not rtn:
|
||||
return None
|
||||
else:
|
||||
return rtn.path
|
||||
|
||||
def getPathThroughPoses(self, start, goals, planner_id='', use_start=False):
|
||||
"""Send a `ComputePathThroughPoses` action request."""
|
||||
self.debug("Waiting for 'ComputePathThroughPoses' action server")
|
||||
while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'ComputePathThroughPoses' action server not available, waiting...")
|
||||
|
||||
goal_msg = ComputePathThroughPoses.Goal()
|
||||
goal_msg.start = start
|
||||
goal_msg.goals = goals
|
||||
goal_msg.planner_id = planner_id
|
||||
goal_msg.use_start = use_start
|
||||
|
||||
self.info('Getting path...')
|
||||
send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error('Get path was rejected!')
|
||||
return None
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
rclpy.spin_until_future_complete(self, self.result_future)
|
||||
self.status = self.result_future.result().status
|
||||
if self.status != GoalStatus.STATUS_SUCCEEDED:
|
||||
self.warn(f'Getting path failed with status code: {self.status}')
|
||||
return None
|
||||
|
||||
return self.result_future.result().result.path
|
||||
|
||||
def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
|
||||
"""
|
||||
Send a `SmoothPath` action request.
|
||||
|
||||
Internal implementation to get the full result, not just the path.
|
||||
"""
|
||||
self.debug("Waiting for 'SmoothPath' action server")
|
||||
while not self.smoother_client.wait_for_server(timeout_sec=1.0):
|
||||
self.info("'SmoothPath' action server not available, waiting...")
|
||||
|
||||
goal_msg = SmoothPath.Goal()
|
||||
goal_msg.path = path
|
||||
goal_msg.max_smoothing_duration = rclpyDuration(seconds=max_duration).to_msg()
|
||||
goal_msg.smoother_id = smoother_id
|
||||
goal_msg.check_for_collisions = check_for_collision
|
||||
|
||||
self.info('Smoothing path...')
|
||||
send_goal_future = self.smoother_client.send_goal_async(goal_msg)
|
||||
rclpy.spin_until_future_complete(self, send_goal_future)
|
||||
self.goal_handle = send_goal_future.result()
|
||||
|
||||
if not self.goal_handle.accepted:
|
||||
self.error('Smooth path was rejected!')
|
||||
return None
|
||||
|
||||
self.result_future = self.goal_handle.get_result_async()
|
||||
rclpy.spin_until_future_complete(self, self.result_future)
|
||||
self.status = self.result_future.result().status
|
||||
if self.status != GoalStatus.STATUS_SUCCEEDED:
|
||||
self.warn(f'Getting path failed with status code: {self.status}')
|
||||
return None
|
||||
|
||||
return self.result_future.result().result
|
||||
|
||||
def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
|
||||
"""Send a `SmoothPath` action request."""
|
||||
rtn = self._smoothPathImpl(
|
||||
path, smoother_id, max_duration, check_for_collision)
|
||||
if not rtn:
|
||||
return None
|
||||
else:
|
||||
return rtn.path
|
||||
|
||||
def changeMap(self, map_filepath):
|
||||
"""Change the current static map in the map server."""
|
||||
while not self.change_maps_srv.wait_for_service(timeout_sec=1.0):
|
||||
self.info('change map service not available, waiting...')
|
||||
req = LoadMap.Request()
|
||||
req.map_url = map_filepath
|
||||
future = self.change_maps_srv.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
status = future.result().result
|
||||
if status != LoadMap.Response().RESULT_SUCCESS:
|
||||
self.error('Change map request failed!')
|
||||
else:
|
||||
self.info('Change map request was successful!')
|
||||
return
|
||||
|
||||
def clearAllCostmaps(self):
|
||||
"""Clear all costmaps."""
|
||||
self.clearLocalCostmap()
|
||||
self.clearGlobalCostmap()
|
||||
return
|
||||
|
||||
def clearLocalCostmap(self):
|
||||
"""Clear local costmap."""
|
||||
while not self.clear_costmap_local_srv.wait_for_service(timeout_sec=1.0):
|
||||
self.info('Clear local costmaps service not available, waiting...')
|
||||
req = ClearEntireCostmap.Request()
|
||||
future = self.clear_costmap_local_srv.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
return
|
||||
|
||||
def clearGlobalCostmap(self):
|
||||
"""Clear global costmap."""
|
||||
while not self.clear_costmap_global_srv.wait_for_service(timeout_sec=1.0):
|
||||
self.info('Clear global costmaps service not available, waiting...')
|
||||
req = ClearEntireCostmap.Request()
|
||||
future = self.clear_costmap_global_srv.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
return
|
||||
|
||||
def getGlobalCostmap(self):
|
||||
"""Get the global costmap."""
|
||||
while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0):
|
||||
self.info('Get global costmaps service not available, waiting...')
|
||||
req = GetCostmap.Request()
|
||||
future = self.get_costmap_global_srv.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
return future.result().map
|
||||
|
||||
def getLocalCostmap(self):
|
||||
"""Get the local costmap."""
|
||||
while not self.get_costmap_local_srv.wait_for_service(timeout_sec=1.0):
|
||||
self.info('Get local costmaps service not available, waiting...')
|
||||
req = GetCostmap.Request()
|
||||
future = self.get_costmap_local_srv.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
return future.result().map
|
||||
|
||||
def lifecycleStartup(self):
|
||||
"""Startup nav2 lifecycle system."""
|
||||
self.info('Starting up lifecycle nodes based on lifecycle_manager.')
|
||||
for srv_name, srv_type in self.get_service_names_and_types():
|
||||
if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
|
||||
self.info(f'Starting up {srv_name}')
|
||||
mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
|
||||
while not mgr_client.wait_for_service(timeout_sec=1.0):
|
||||
self.info(f'{srv_name} service not available, waiting...')
|
||||
req = ManageLifecycleNodes.Request()
|
||||
req.command = ManageLifecycleNodes.Request().STARTUP
|
||||
future = mgr_client.call_async(req)
|
||||
|
||||
# starting up requires a full map->odom->base_link TF tree
|
||||
# so if we're not successful, try forwarding the initial pose
|
||||
while True:
|
||||
rclpy.spin_until_future_complete(self, future, timeout_sec=0.10)
|
||||
if not future:
|
||||
self._waitForInitialPose()
|
||||
else:
|
||||
break
|
||||
self.info('Nav2 is ready for use!')
|
||||
return
|
||||
|
||||
def lifecycleShutdown(self):
|
||||
"""Shutdown nav2 lifecycle system."""
|
||||
self.info('Shutting down lifecycle nodes based on lifecycle_manager.')
|
||||
for srv_name, srv_type in self.get_service_names_and_types():
|
||||
if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
|
||||
self.info(f'Shutting down {srv_name}')
|
||||
mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
|
||||
while not mgr_client.wait_for_service(timeout_sec=1.0):
|
||||
self.info(f'{srv_name} service not available, waiting...')
|
||||
req = ManageLifecycleNodes.Request()
|
||||
req.command = ManageLifecycleNodes.Request().SHUTDOWN
|
||||
future = mgr_client.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
future.result()
|
||||
return
|
||||
|
||||
def _waitForNodeToActivate(self, node_name):
|
||||
# Waits for the node within the tester namespace to become active
|
||||
self.debug(f'Waiting for {node_name} to become active..')
|
||||
node_service = f'{node_name}/get_state'
|
||||
state_client = self.create_client(GetState, node_service)
|
||||
while not state_client.wait_for_service(timeout_sec=1.0):
|
||||
self.info(f'{node_service} service not available, waiting...')
|
||||
|
||||
req = GetState.Request()
|
||||
state = 'unknown'
|
||||
while state != 'active':
|
||||
self.debug(f'Getting {node_name} state...')
|
||||
future = state_client.call_async(req)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
if future.result() is not None:
|
||||
state = future.result().current_state.label
|
||||
self.debug(f'Result of get_state: {state}')
|
||||
time.sleep(2)
|
||||
return
|
||||
|
||||
def _waitForInitialPose(self):
|
||||
while not self.initial_pose_received:
|
||||
self.info('Setting initial pose')
|
||||
self._setInitialPose()
|
||||
self.info('Waiting for amcl_pose to be received')
|
||||
rclpy.spin_once(self, timeout_sec=1.0)
|
||||
return
|
||||
|
||||
def _amclPoseCallback(self, msg):
|
||||
self.debug('Received amcl pose')
|
||||
self.initial_pose_received = True
|
||||
return
|
||||
|
||||
def _feedbackCallback(self, msg):
|
||||
self.debug('Received action feedback message')
|
||||
self.feedback = msg.feedback
|
||||
return
|
||||
|
||||
def _setInitialPose(self):
|
||||
msg = PoseWithCovarianceStamped()
|
||||
msg.pose.pose = self.initial_pose.pose
|
||||
msg.header.frame_id = self.initial_pose.header.frame_id
|
||||
msg.header.stamp = self.initial_pose.header.stamp
|
||||
self.info('Publishing Initial Pose')
|
||||
self.initial_pose_pub.publish(msg)
|
||||
return
|
||||
|
||||
def info(self, msg):
|
||||
self.get_logger().info(msg)
|
||||
return
|
||||
|
||||
def warn(self, msg):
|
||||
self.get_logger().warn(msg)
|
||||
return
|
||||
|
||||
def error(self, msg):
|
||||
self.get_logger().error(msg)
|
||||
return
|
||||
|
||||
def debug(self, msg):
|
||||
self.get_logger().debug(msg)
|
||||
return
|
||||
@@ -0,0 +1,24 @@
|
||||
<?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>nav2_simple_commander</name>
|
||||
<version>1.1.18</version>
|
||||
<description>An importable library for writing mobile robot applications in python3</description>
|
||||
<maintainer email="stevenmacenski@gmail.com">steve</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>nav2_msgs</exec_depend>
|
||||
<exec_depend>action_msgs</exec_depend>
|
||||
<exec_depend>lifecycle_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,2 @@
|
||||
[pytest]
|
||||
junit_family=xunit2
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/nav2_simple_commander
|
||||
[install]
|
||||
install_scripts=$base/lib/nav2_simple_commander
|
||||
@@ -0,0 +1,39 @@
|
||||
from glob import glob
|
||||
import os
|
||||
|
||||
from setuptools import setup
|
||||
|
||||
|
||||
package_name = 'nav2_simple_commander'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='1.0.0',
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name), glob('launch/*')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='steve',
|
||||
maintainer_email='stevenmacenski@gmail.com',
|
||||
description='An importable library for writing mobile robot applications in python3',
|
||||
license='Apache-2.0',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'example_nav_to_pose = nav2_simple_commander.example_nav_to_pose:main',
|
||||
'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main',
|
||||
'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main',
|
||||
'example_follow_path = nav2_simple_commander.example_follow_path:main',
|
||||
'demo_picking = nav2_simple_commander.demo_picking:main',
|
||||
'demo_inspection = nav2_simple_commander.demo_inspection:main',
|
||||
'demo_security = nav2_simple_commander.demo_security:main',
|
||||
'demo_recoveries = nav2_simple_commander.demo_recoveries:main',
|
||||
'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,23 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
@@ -0,0 +1,25 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
@@ -0,0 +1,107 @@
|
||||
# Copyright 2022 Afif Swaidan
|
||||
#
|
||||
# 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 unittest
|
||||
from cmath import sqrt
|
||||
from nav2_simple_commander.line_iterator import LineIterator
|
||||
|
||||
|
||||
class TestLineIterator(unittest.TestCase):
|
||||
|
||||
def test_type_error(self):
|
||||
# Test if a type error raised when passing invalid arguements types
|
||||
self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1')
|
||||
|
||||
def test_value_error(self):
|
||||
# Test if a value error raised when passing negative or zero step_size
|
||||
self.assertRaises(ValueError, LineIterator, 0, 0, 10, 10, -2)
|
||||
# Test if a value error raised when passing zero length line
|
||||
self.assertRaises(ValueError, LineIterator, 2, 2, 2, 2, 1)
|
||||
|
||||
def test_get_xy(self):
|
||||
# Test if the initial and final coordinates are returned correctly
|
||||
lt = LineIterator(0, 0, 5, 5, 1)
|
||||
self.assertEqual(lt.getX0(), 0)
|
||||
self.assertEqual(lt.getY0(), 0)
|
||||
self.assertEqual(lt.getX1(), 5)
|
||||
self.assertEqual(lt.getY1(), 5)
|
||||
|
||||
def test_line_length(self):
|
||||
# Test if the line length is calculated correctly
|
||||
lt = LineIterator(0, 0, 5, 5, 1)
|
||||
self.assertEqual(lt.get_line_length(), sqrt(pow(5, 2) + pow(5, 2)))
|
||||
|
||||
def test_straight_line(self):
|
||||
# Test if the calculations are correct for y = x
|
||||
lt = LineIterator(0, 0, 5, 5, 1)
|
||||
i = 0
|
||||
while lt.isValid():
|
||||
self.assertEqual(lt.getX(), lt.getX0() + i)
|
||||
self.assertEqual(lt.getY(), lt.getY0() + i)
|
||||
lt.advance()
|
||||
i += 1
|
||||
|
||||
# Test if the calculations are correct for y = 2x (positive slope)
|
||||
lt = LineIterator(0, 0, 5, 10, 1)
|
||||
i = 0
|
||||
while lt.isValid():
|
||||
self.assertEqual(lt.getX(), lt.getX0() + i)
|
||||
self.assertEqual(lt.getY(), lt.getY0() + (i*2))
|
||||
lt.advance()
|
||||
i += 1
|
||||
|
||||
# Test if the calculations are correct for y = -2x (negative slope)
|
||||
lt = LineIterator(0, 0, 5, -10, 1)
|
||||
i = 0
|
||||
while lt.isValid():
|
||||
self.assertEqual(lt.getX(), lt.getX0() + i)
|
||||
self.assertEqual(lt.getY(), lt.getY0() + (-i*2))
|
||||
lt.advance()
|
||||
i += 1
|
||||
|
||||
def test_hor_line(self):
|
||||
# Test if the calculations are correct for y = 0x+b (horizontal line)
|
||||
lt = LineIterator(0, 10, 5, 10, 1)
|
||||
i = 0
|
||||
while lt.isValid():
|
||||
self.assertEqual(lt.getX(), lt.getX0() + i)
|
||||
self.assertEqual(lt.getY(), lt.getY0())
|
||||
lt.advance()
|
||||
i += 1
|
||||
|
||||
def test_ver_line(self):
|
||||
# Test if the calculations are correct for x = n (vertical line)
|
||||
lt = LineIterator(5, 0, 5, 10, 1)
|
||||
i = 0
|
||||
while lt.isValid():
|
||||
self.assertEqual(lt.getX(), lt.getX0())
|
||||
self.assertEqual(lt.getY(), lt.getY0() + i)
|
||||
lt.advance()
|
||||
i += 1
|
||||
|
||||
def test_clamp(self):
|
||||
# Test if the increments are clamped to avoid crossing the final points
|
||||
# when step_size is large with respect to line length
|
||||
lt = LineIterator(0, 0, 5, 5, 10)
|
||||
self.assertEqual(lt.getX(), 0)
|
||||
self.assertEqual(lt.getY(), 0)
|
||||
lt.advance()
|
||||
while lt.isValid():
|
||||
self.assertEqual(lt.getX(), 5)
|
||||
self.assertEqual(lt.getY(), 5)
|
||||
lt.advance()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,23 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
Reference in New Issue
Block a user