From bca282813abf65521167d07eebc7b6f39e4c0258 Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Sat, 27 Apr 2024 23:58:22 +0900 Subject: [PATCH] feat: add license (#6) --- .../navyu_costmap_2d/navyu_costmap_2d.hpp | 14 ++ .../plugins/inflation_layer.hpp | 14 ++ .../navyu_costmap_2d/plugins/layer.hpp | 14 ++ .../navyu_costmap_2d/plugins/static_layer.hpp | 14 ++ .../launch/navyu_costmap_2d.launch.py | 14 ++ navyu_costmap_2d/src/navyu_costmap_2d.cpp | 14 ++ .../launch/localization.launch.py | 134 ++++++----- .../launch/navigation2_bringup.launch.py | 129 +++++----- .../include/navyu_planner/astar_planner.hpp | 14 ++ .../navyu_planner/base_global_planner.hpp | 14 ++ .../navyu_planner/navyu_global_planner.hpp | 14 ++ navyu_planner/include/navyu_planner/node.hpp | 14 ++ .../launch/navyu_global_planner.launch.py | 14 ++ navyu_planner/src/navyu_global_planner.cpp | 14 ++ .../navyu_simple_simulator.hpp | 20 +- .../navyu_simulator/quaternion_utils.hpp | 20 +- .../navyu_simple_simulator_bringup.launch.py | 159 +++++++------ .../launch/navyu_simulator_bringup.launch.py | 221 ++++++++++-------- .../src/navyu_simple_simulator.cpp | 14 ++ 19 files changed, 575 insertions(+), 290 deletions(-) diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp index ccf46ec..d432067 100644 --- a/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp +++ b/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #ifndef NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_ #define NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_ diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp index 5bb46b3..46125f2 100644 --- a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #ifndef NAVYU_COSTMAP_2D__PLUGINS__INFLATION_LAYER_HPP_ #define NAVYU_COSTMAP_2D__PLUGINS__INFLATION_LAYER_HPP_ diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp index d05f67a..59cc1e6 100644 --- a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #ifndef NAVYU_COSTMAP_2D__PLUGINS__LAYER_HPP_ #define NAVYU_COSTMAP_2D__PLUGINS__LAYER_HPP_ diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp index 02f2a54..b8737e3 100644 --- a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #ifndef NAVYU_COSTMAP_2D__PLUGINS__STATIC_LAYER_HPP_ #define NAVYU_COSTMAP_2D__PLUGINS__STATIC_LAYER_HPP_ diff --git a/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py b/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py index 2a03445..c974650 100644 --- a/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py +++ b/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py @@ -1,3 +1,17 @@ +# Copyright 2024 RyuYamamoto. +# +# 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 launch_ros.actions import Node from launch import LaunchDescription diff --git a/navyu_costmap_2d/src/navyu_costmap_2d.cpp b/navyu_costmap_2d/src/navyu_costmap_2d.cpp index a9cf767..6adb9e2 100644 --- a/navyu_costmap_2d/src/navyu_costmap_2d.cpp +++ b/navyu_costmap_2d/src/navyu_costmap_2d.cpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #include "navyu_costmap_2d/navyu_costmap_2d.hpp" #include "navyu_costmap_2d/plugins/inflation_layer.hpp" diff --git a/navyu_navigation/launch/localization.launch.py b/navyu_navigation/launch/localization.launch.py index df974b9..75c7312 100644 --- a/navyu_navigation/launch/localization.launch.py +++ b/navyu_navigation/launch/localization.launch.py @@ -1,3 +1,17 @@ +# Copyright 2024 RyuYamamoto. +# +# 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 @@ -6,75 +20,77 @@ from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node + def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time') - amcl_config_path = LaunchConfiguration('amcl_config_path') - map_path = LaunchConfiguration('map_path') + use_sim_time = LaunchConfiguration("use_sim_time") + amcl_config_path = LaunchConfiguration("amcl_config_path") + map_path = LaunchConfiguration("map_path") - costmap_config_path = PathJoinSubstitution( - [FindPackageShare('navyu_navigation'), - 'config', 'global_costmap_params.yaml'] - ) + costmap_config_path = PathJoinSubstitution( + [FindPackageShare("navyu_navigation"), "config", "global_costmap_params.yaml"] + ) - use_sim_time_cmd = DeclareLaunchArgument( - 'use_sim_time', default_value='true', description='' - ) + use_sim_time_cmd = DeclareLaunchArgument("use_sim_time", default_value="true", description="") - amcl_config_path_cmd = DeclareLaunchArgument( - 'amcl_config_path', - default_value=PathJoinSubstitution([FindPackageShare('navyu_navigation'), 'config', 'amcl_params.yaml']), - description='' - ) + amcl_config_path_cmd = DeclareLaunchArgument( + "amcl_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("navyu_navigation"), "config", "amcl_params.yaml"] + ), + description="", + ) - map_path_cmd = DeclareLaunchArgument( - 'map_path', - default_value=PathJoinSubstitution([FindPackageShare('navyu_navigation'), 'map', 'map.yaml']) - ) + map_path_cmd = DeclareLaunchArgument( + "map_path", + default_value=PathJoinSubstitution( + [FindPackageShare("navyu_navigation"), "map", "map.yaml"] + ), + ) - amcl_node = Node( - package='nav2_amcl', - executable='amcl', - name='amcl', - output='screen', - parameters=[amcl_config_path, {'use_sim_time': use_sim_time}], - ) + amcl_node = Node( + package="nav2_amcl", + executable="amcl", + name="amcl", + output="screen", + parameters=[amcl_config_path, {"use_sim_time": use_sim_time}], + ) - map_server_node = Node( - package='nav2_map_server', - executable='map_server', - name='map_server', - parameters=[{'yaml_filename': map_path}, {'frame_id': 'map'}] - ) - - lifecycle_node = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager', - output='screen', - emulate_tty=True, - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': True}, - {'node_names': ['map_server', 'amcl']}] - ) + map_server_node = Node( + package="nav2_map_server", + executable="map_server", + name="map_server", + parameters=[{"yaml_filename": map_path}, {"frame_id": "map"}], + ) - rviz_config = os.path.join(get_package_share_directory('navyu_navigation'), 'rviz', 'navigation.rviz') - rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config] - ) + lifecycle_node = Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager", + output="screen", + emulate_tty=True, + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": True}, + {"node_names": ["map_server", "amcl"]}, + ], + ) + rviz_config = os.path.join( + get_package_share_directory("navyu_navigation"), "rviz", "navigation.rviz" + ) + rviz_node = Node( + package="rviz2", executable="rviz2", name="rviz2", arguments=["-d", rviz_config] + ) - ld = LaunchDescription() + ld = LaunchDescription() - ld.add_action(use_sim_time_cmd) - ld.add_action(amcl_config_path_cmd) - ld.add_action(map_path_cmd) - ld.add_action(amcl_node) - ld.add_action(map_server_node) - ld.add_action(lifecycle_node) - ld.add_action(rviz_node) + ld.add_action(use_sim_time_cmd) + ld.add_action(amcl_config_path_cmd) + ld.add_action(map_path_cmd) + ld.add_action(amcl_node) + ld.add_action(map_server_node) + ld.add_action(lifecycle_node) + ld.add_action(rviz_node) - return ld + return ld diff --git a/navyu_navigation/launch/navigation2_bringup.launch.py b/navyu_navigation/launch/navigation2_bringup.launch.py index bd8c8b1..0d4f3a4 100644 --- a/navyu_navigation/launch/navigation2_bringup.launch.py +++ b/navyu_navigation/launch/navigation2_bringup.launch.py @@ -1,3 +1,17 @@ +# Copyright 2024 RyuYamamoto. +# +# 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 launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription @@ -9,72 +23,67 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -def generate_launch_description(): - - use_sim = LaunchConfiguration('use_sim') - autostart = LaunchConfiguration('autostart') - use_composition = LaunchConfiguration('use_composition') - use_respawn = LaunchConfiguration('use_respawn') - - map_file_path = PathJoinSubstitution( - [FindPackageShare('navyu_navigation'), 'map', 'map.yaml'] - ) - - nav2_config_path = PathJoinSubstitution( - [FindPackageShare('navyu_navigation'), 'config', 'nav2_params.yaml'] - ) - nav2_launch_path = PathJoinSubstitution( - [FindPackageShare('nav2_bringup'), 'launch'] - ) - - default_bt_xml_filename = PathJoinSubstitution( - [FindPackageShare('nav2_bt_navigator'), 'behavior_trees', 'navigate_w_resplanning_and_recovery.xml'] - ) +def generate_launch_description(): - rviz_config = PathJoinSubstitution( - [FindPackageShare('navyu_navigation'), 'rviz', 'navigation.rviz'] - ) + use_sim = LaunchConfiguration("use_sim") + autostart = LaunchConfiguration("autostart") + use_composition = LaunchConfiguration("use_composition") + use_respawn = LaunchConfiguration("use_respawn") - return LaunchDescription([ - DeclareLaunchArgument( - 'use_sim', - default_value='true', - description='Start robot in Gazebo simulation'), + map_file_path = PathJoinSubstitution([FindPackageShare("navyu_navigation"), "map", "map.yaml"]) - DeclareLaunchArgument( - 'autostart', - default_value='true', - description='Automatically startup the nav2 stack'), + nav2_config_path = PathJoinSubstitution( + [FindPackageShare("navyu_navigation"), "config", "nav2_params.yaml"] + ) - DeclareLaunchArgument( - 'use_composition', - default_value='True', - description='Whether to use composed bringup'), + nav2_launch_path = PathJoinSubstitution([FindPackageShare("nav2_bringup"), "launch"]) - DeclareLaunchArgument( - 'use_respawn', - default_value='false', - description='Whether to respawn if a node crashes. \ - Applied when composition is disabled.'), + default_bt_xml_filename = PathJoinSubstitution( + [ + FindPackageShare("nav2_bt_navigator"), + "behavior_trees", + "navigate_w_resplanning_and_recovery.xml", + ] + ) - IncludeLaunchDescription( - PythonLaunchDescriptionSource([nav2_launch_path, '/bringup_launch.py']), - launch_arguments={ - 'map': map_file_path, - 'use_sim_time': use_sim, - 'params_file': nav2_config_path, - #'default_bt_xml_filename': default_bt_xml_filename, - 'autostart': autostart, - 'use_composition': use_composition, - 'use_respawn': use_respawn - }.items() - ), + rviz_config = PathJoinSubstitution( + [FindPackageShare("navyu_navigation"), "rviz", "navigation.rviz"] + ) - Node( - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config] + return LaunchDescription( + [ + DeclareLaunchArgument( + "use_sim", default_value="true", description="Start robot in Gazebo simulation" + ), + DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically startup the nav2 stack", + ), + DeclareLaunchArgument( + "use_composition", + default_value="True", + description="Whether to use composed bringup", + ), + DeclareLaunchArgument( + "use_respawn", + default_value="false", + description="Whether to respawn if a node crashes. \ + Applied when composition is disabled.", + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_launch_path, "/bringup_launch.py"]), + launch_arguments={ + "map": map_file_path, + "use_sim_time": use_sim, + "params_file": nav2_config_path, + #'default_bt_xml_filename': default_bt_xml_filename, + "autostart": autostart, + "use_composition": use_composition, + "use_respawn": use_respawn, + }.items(), + ), + Node(package="rviz2", executable="rviz2", name="rviz2", arguments=["-d", rviz_config]), + ] ) - ]) diff --git a/navyu_planner/include/navyu_planner/astar_planner.hpp b/navyu_planner/include/navyu_planner/astar_planner.hpp index abddb80..20899ed 100644 --- a/navyu_planner/include/navyu_planner/astar_planner.hpp +++ b/navyu_planner/include/navyu_planner/astar_planner.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #ifndef NAVYU_PLANNER__ASTAR_PLANNER_HPP_ #define NAVYU_PLANNER__ASTAR_PLANNER_HPP_ diff --git a/navyu_planner/include/navyu_planner/base_global_planner.hpp b/navyu_planner/include/navyu_planner/base_global_planner.hpp index c5b1ceb..cd41b7f 100644 --- a/navyu_planner/include/navyu_planner/base_global_planner.hpp +++ b/navyu_planner/include/navyu_planner/base_global_planner.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #ifndef NAVYU_PLANNER__BASE_GLOBAL_PLANNER_HPP_ #define NAVYU_PLANNER__BASE_GLOBAL_PLANNER_HPP_ diff --git a/navyu_planner/include/navyu_planner/navyu_global_planner.hpp b/navyu_planner/include/navyu_planner/navyu_global_planner.hpp index 2e3bb4a..9a77f7d 100644 --- a/navyu_planner/include/navyu_planner/navyu_global_planner.hpp +++ b/navyu_planner/include/navyu_planner/navyu_global_planner.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #ifndef NAVYU_PLANNER__NAVYU_GLOBAL_PLANNER_HPP_ #define NAVYU_PLANNER__NAVYU_GLOBAL_PLANNER_HPP_ diff --git a/navyu_planner/include/navyu_planner/node.hpp b/navyu_planner/include/navyu_planner/node.hpp index cbea2cf..e8d3710 100644 --- a/navyu_planner/include/navyu_planner/node.hpp +++ b/navyu_planner/include/navyu_planner/node.hpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #ifndef NAVYU_PLANNER__NODE_HPP_ #define NAVYU_PLANNER__NODE_HPP_ diff --git a/navyu_planner/launch/navyu_global_planner.launch.py b/navyu_planner/launch/navyu_global_planner.launch.py index 6b8b984..a9971fa 100644 --- a/navyu_planner/launch/navyu_global_planner.launch.py +++ b/navyu_planner/launch/navyu_global_planner.launch.py @@ -1,3 +1,17 @@ +# Copyright 2024 RyuYamamoto. +# +# 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 launch_ros.actions import Node from launch import LaunchDescription diff --git a/navyu_planner/src/navyu_global_planner.cpp b/navyu_planner/src/navyu_global_planner.cpp index 5be4d8e..641775a 100644 --- a/navyu_planner/src/navyu_global_planner.cpp +++ b/navyu_planner/src/navyu_global_planner.cpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #include "navyu_planner/navyu_global_planner.hpp" #include diff --git a/navyu_simulator/include/navyu_simulator/navyu_simple_simulator.hpp b/navyu_simulator/include/navyu_simulator/navyu_simple_simulator.hpp index 3c0dcea..286aa76 100644 --- a/navyu_simulator/include/navyu_simulator/navyu_simple_simulator.hpp +++ b/navyu_simulator/include/navyu_simulator/navyu_simple_simulator.hpp @@ -1,5 +1,19 @@ -#ifndef _NAVYU_SIMULATOR__NAVYU_SIMPLE_SIMULATOR_HPP_ -#define _NAVYU_SIMULATOR__NAVYU_SIMPLE_SIMULATOR_HPP_ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + +#ifndef NAVYU_SIMULATOR__NAVYU_SIMPLE_SIMULATOR_HPP_ +#define NAVYU_SIMULATOR__NAVYU_SIMPLE_SIMULATOR_HPP_ #include #include @@ -65,4 +79,4 @@ class NavyuSimpleSimulator : public rclcpp::Node std::vector joint_names_; }; -#endif +#endif // NAVYU_SIMULATOR__NAVYU_SIMPLE_SIMULATOR_HPP_ diff --git a/navyu_simulator/include/navyu_simulator/quaternion_utils.hpp b/navyu_simulator/include/navyu_simulator/quaternion_utils.hpp index ede029d..2e36931 100644 --- a/navyu_simulator/include/navyu_simulator/quaternion_utils.hpp +++ b/navyu_simulator/include/navyu_simulator/quaternion_utils.hpp @@ -1,5 +1,19 @@ -#ifndef _NAVYU_SIMULATOR__QUATERNION_UTILS_HPP_ -#define _NAVYU_SIMULATOR__QUATERNION_UTILS_HPP_ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + +#ifndef NAVYU_SIMULATOR__QUATERNION_UTILS_HPP_ +#define NAVYU_SIMULATOR__QUATERNION_UTILS_HPP_ #include #include @@ -70,4 +84,4 @@ Eigen::Matrix3f get_rotation_matrix_from_quaternion(const geometry_msgs::msg::Qu } // namespace quaternion_utils -#endif +#endif // NAVYU_SIMULATOR__QUATERNION_UTILS_HPP_ diff --git a/navyu_simulator/launch/navyu_simple_simulator_bringup.launch.py b/navyu_simulator/launch/navyu_simple_simulator_bringup.launch.py index b164145..982fd2f 100644 --- a/navyu_simulator/launch/navyu_simple_simulator_bringup.launch.py +++ b/navyu_simulator/launch/navyu_simple_simulator_bringup.launch.py @@ -1,3 +1,17 @@ +# Copyright 2024 RyuYamamoto. +# +# 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 import launch_ros @@ -13,78 +27,77 @@ from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - use_rviz = LaunchConfiguration('use_rviz', default='true') - - urdf_file = os.path.join(get_package_share_directory('navyu_simulator'), 'urdf', 'sample_robot.urdf') - - simple_simulator_config_path = os.path.join(get_package_share_directory('navyu_simulator'), 'config', 'simple_simulator_params.yaml') - - robot_description = launch_ros.descriptions.ParameterValue( - Command([ - 'xacro', ' ', urdf_file]), - value_type=str) - - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[{'robot_description': robot_description}, {'use_sim_time': use_sim_time}] - ) - - simple_simulator_node = Node( - package='navyu_simulator', - executable='navyu_simulator_node', - #name='navyu_simulator_node', - output='screen', - parameters=[simple_simulator_config_path, {'use_sim_time': use_sim_time}], - ) - - map_path = PathJoinSubstitution( - [FindPackageShare('navyu_navigation'), - 'map', 'map.yaml'] - ) - - map_server_node = Node( - package='nav2_map_server', - executable='map_server', - name='map_server', - parameters=[{'yaml_filename': map_path}, - {'frame_id': 'map'}] - ) - - lifesycle_manager_node = Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='map_server_lifecycle_manager', - output='screen', - emulate_tty=True, - parameters=[{'use_sim_time': True}, - {'autostart': True}, - {'node_names': ['map_server']}] - ) - - - use_rviz_arg = DeclareLaunchArgument('use_rviz', default_value='true') - - rviz_config = os.path.join(get_package_share_directory('navyu_simulator'), 'rviz', 'simple_simulator.rviz') - rviz_node = Node( - condition=IfCondition(LaunchConfiguration('use_rviz')), - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config] - ) - - ld = LaunchDescription() - - ld.add_action(use_rviz_arg) - ld.add_action(simple_simulator_node) - ld.add_action(robot_state_publisher) - ld.add_action(map_server_node) - ld.add_action(lifesycle_manager_node) - ld.add_action(rviz_node) - - return ld + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + use_rviz = LaunchConfiguration("use_rviz", default="true") + + urdf_file = os.path.join( + get_package_share_directory("navyu_simulator"), "urdf", "sample_robot.urdf" + ) + + simple_simulator_config_path = os.path.join( + get_package_share_directory("navyu_simulator"), "config", "simple_simulator_params.yaml" + ) + + robot_description = launch_ros.descriptions.ParameterValue( + Command(["xacro", " ", urdf_file]), value_type=str + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}, {"use_sim_time": use_sim_time}], + ) + + simple_simulator_node = Node( + package="navyu_simulator", + executable="navyu_simulator_node", + # name='navyu_simulator_node', + output="screen", + parameters=[simple_simulator_config_path, {"use_sim_time": use_sim_time}], + ) + + map_path = PathJoinSubstitution([FindPackageShare("navyu_navigation"), "map", "map.yaml"]) + + map_server_node = Node( + package="nav2_map_server", + executable="map_server", + name="map_server", + parameters=[{"yaml_filename": map_path}, {"frame_id": "map"}], + ) + + lifesycle_manager_node = Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="map_server_lifecycle_manager", + output="screen", + emulate_tty=True, + parameters=[{"use_sim_time": True}, {"autostart": True}, {"node_names": ["map_server"]}], + ) + + use_rviz_arg = DeclareLaunchArgument("use_rviz", default_value="true") + + rviz_config = os.path.join( + get_package_share_directory("navyu_simulator"), "rviz", "simple_simulator.rviz" + ) + rviz_node = Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config], + ) + + ld = LaunchDescription() + + ld.add_action(use_rviz_arg) + ld.add_action(simple_simulator_node) + ld.add_action(robot_state_publisher) + ld.add_action(map_server_node) + ld.add_action(lifesycle_manager_node) + ld.add_action(rviz_node) + + return ld diff --git a/navyu_simulator/launch/navyu_simulator_bringup.launch.py b/navyu_simulator/launch/navyu_simulator_bringup.launch.py index 3f2f19e..68a10f5 100644 --- a/navyu_simulator/launch/navyu_simulator_bringup.launch.py +++ b/navyu_simulator/launch/navyu_simulator_bringup.launch.py @@ -1,3 +1,17 @@ +# Copyright 2024 RyuYamamoto. +# +# 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 os import pathsep @@ -10,102 +24,125 @@ from launch.actions import IncludeLaunchDescription from launch.conditions import IfCondition from launch.substitutions import Command -from launch.substitutions import (EnvironmentVariable, PathJoinSubstitution) +from launch.substitutions import EnvironmentVariable, PathJoinSubstitution from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - use_rviz = LaunchConfiguration('use_rviz', default='false') - - urdf_file = os.path.join(get_package_share_directory('navyu_simulator'), 'urdf', 'sample_robot.urdf') - world_file = os.path.join(get_package_share_directory('navyu_simulator'), 'world', 'willow_garage.world') - - robot_description = launch_ros.descriptions.ParameterValue( - Command([ - 'xacro', ' ', urdf_file, ' ', 'use_ros2_control:=true', ' ', 'sim_mode:=true', ' ', 'gazebo:=true']), - value_type=str) - - set_env_gazebo_resource = SetEnvironmentVariable( - name='GAZEBO_RESOURCE_PATH', - value=[ - EnvironmentVariable('GAZEBO_RESOURCE_PATH', default_value=''), - pathsep, - '/usr/share/gazebo-11', - pathsep, - PathJoinSubstitution([get_package_share_directory('navyu_simulator'), 'world']) - ] - ) - - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[{'robot_description': robot_description}, {'use_sim_time': use_sim_time}] - ) - - world_file_path_arg = DeclareLaunchArgument( - 'world', - default_value=world_file, - description='gazebo world file' - ) - - use_rviz_arg = DeclareLaunchArgument('use_rviz', default_value='false') - - gazebo_ros_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py'), - ), - ) - - spawn_node = Node( - package='gazebo_ros', - executable='spawn_entity.py', - parameters=[{'use_sim_time': use_sim_time}], - arguments=[ - "-topic", "/robot_description", - "-entity", "sample_robot", - "-x", "-9", - "-y", "-17", - "-z", "0.88", - "-Y", "1.57", - ] - ) - - joint_state_broadcaster_node = Node( - package='controller_manager', - executable='spawner', - arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'] - ) - - diff_drive_controller_node = Node( - package='controller_manager', - executable='spawner', - arguments=['diff_drive_controller', '--controller-manager', '/controller_manager'] - ) - - rviz_config = os.path.join(get_package_share_directory('navyu_simulator'), 'rviz', 'default.rviz') - rviz_node = Node( - condition=IfCondition(LaunchConfiguration('use_rviz')), - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config] - ) - - ld = LaunchDescription() - - ld.add_action(use_rviz_arg) - ld.add_action(world_file_path_arg) - ld.add_action(robot_state_publisher) - ld.add_action(spawn_node) - ld.add_action(gazebo_ros_cmd) - ld.add_action(set_env_gazebo_resource) - ld.add_action(joint_state_broadcaster_node) - ld.add_action(diff_drive_controller_node) - ld.add_action(rviz_node) - - return ld + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + use_rviz = LaunchConfiguration("use_rviz", default="false") + + urdf_file = os.path.join( + get_package_share_directory("navyu_simulator"), "urdf", "sample_robot.urdf" + ) + world_file = os.path.join( + get_package_share_directory("navyu_simulator"), "world", "willow_garage.world" + ) + + robot_description = launch_ros.descriptions.ParameterValue( + Command( + [ + "xacro", + " ", + urdf_file, + " ", + "use_ros2_control:=true", + " ", + "sim_mode:=true", + " ", + "gazebo:=true", + ] + ), + value_type=str, + ) + + set_env_gazebo_resource = SetEnvironmentVariable( + name="GAZEBO_RESOURCE_PATH", + value=[ + EnvironmentVariable("GAZEBO_RESOURCE_PATH", default_value=""), + pathsep, + "/usr/share/gazebo-11", + pathsep, + PathJoinSubstitution([get_package_share_directory("navyu_simulator"), "world"]), + ], + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}, {"use_sim_time": use_sim_time}], + ) + + world_file_path_arg = DeclareLaunchArgument( + "world", default_value=world_file, description="gazebo world file" + ) + + use_rviz_arg = DeclareLaunchArgument("use_rviz", default_value="false") + + gazebo_ros_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory("gazebo_ros"), "launch", "gazebo.launch.py"), + ), + ) + + spawn_node = Node( + package="gazebo_ros", + executable="spawn_entity.py", + parameters=[{"use_sim_time": use_sim_time}], + arguments=[ + "-topic", + "/robot_description", + "-entity", + "sample_robot", + "-x", + "-9", + "-y", + "-17", + "-z", + "0.88", + "-Y", + "1.57", + ], + ) + + joint_state_broadcaster_node = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + diff_drive_controller_node = Node( + package="controller_manager", + executable="spawner", + arguments=["diff_drive_controller", "--controller-manager", "/controller_manager"], + ) + + rviz_config = os.path.join( + get_package_share_directory("navyu_simulator"), "rviz", "default.rviz" + ) + rviz_node = Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config], + ) + + ld = LaunchDescription() + + ld.add_action(use_rviz_arg) + ld.add_action(world_file_path_arg) + ld.add_action(robot_state_publisher) + ld.add_action(spawn_node) + ld.add_action(gazebo_ros_cmd) + ld.add_action(set_env_gazebo_resource) + ld.add_action(joint_state_broadcaster_node) + ld.add_action(diff_drive_controller_node) + ld.add_action(rviz_node) + + return ld diff --git a/navyu_simulator/src/navyu_simple_simulator.cpp b/navyu_simulator/src/navyu_simple_simulator.cpp index 4fd6e6d..0a1af48 100644 --- a/navyu_simulator/src/navyu_simple_simulator.cpp +++ b/navyu_simulator/src/navyu_simple_simulator.cpp @@ -1,3 +1,17 @@ +// Copyright 2024 RyuYamamoto. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License + #include "navyu_simulator/navyu_simple_simulator.hpp" #include "navyu_simulator/quaternion_utils.hpp"