diff --git a/simple_commander_api_template.py b/simple_commander_api_template.py new file mode 100644 index 00000000..ab4e8ae1 --- /dev/null +++ b/simple_commander_api_template.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 +import rclpy +from nav2_simple_commander.robot_navigator import BasicNavigator +from geometry_msgs.msg import PoseStamped +import tf_transformations + +def create_pose_stamped(navigator, position_x, position_y, rotation_z): + q_x, q_y, q_z, q_w = tf_transformations.quaternion_from_euler(0.0, 0.0, rotation_z) + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = position_x + goal_pose.pose.position.y = position_y + goal_pose.pose.position.z = 0.0 + goal_pose.pose.orientation.x = q_x + goal_pose.pose.orientation.y = q_y + goal_pose.pose.orientation.z = q_z + goal_pose.pose.orientation.w = q_w + return goal_pose + +def main(): + # --- Init ROS2 communications and Simple Commander API --- + rclpy.init() + nav = BasicNavigator() + + # --- Set initial pose --- + # !!! Comment if the initial pose is already set !!! + initial_pose = create_pose_stamped(nav, 0.0, 0.0, 0.0) + #nav.setInitialPose(initial_pose) + + # --- Wait for Nav2 --- + nav.waitUntilNav2Active() + + # --- Create some Nav2 goal poses --- + goal_pose1 = create_pose_stamped(nav, 3.5, 1.0, 1.57) + goal_pose2 = create_pose_stamped(nav, 2.0, 2.5, 3.14) + goal_pose3 = create_pose_stamped(nav, 0.5, 1.0, 0.0) + + # --- Going to one pose --- + nav.goToPose(goal_pose1) + while not nav.isTaskComplete(): + feedback = nav.getFeedback() + # print(feedback) + + # --- Follow Waypoints --- + # waypoints = [goal_pose1, goal_pose2, goal_pose3] + # for i in range(3): + # nav.followWaypoints(waypoints) + + # while not nav.isTaskComplete(): + # feedback = nav.getFeedback() + # # print(feedback) + + # --- Get the result --- + print(nav.getResult()) + + # --- Shutdown ROS2 communications --- + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/turtlebot3_gazebo/launch/turtlebot3_my_ghar.launch.py b/turtlebot3_gazebo/launch/turtlebot3_my_ghar.launch.py new file mode 100644 index 00000000..0e432504 --- /dev/null +++ b/turtlebot3_gazebo/launch/turtlebot3_my_ghar.launch.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +# +# Copyright 2019 ROBOTIS CO., LTD. +# +# 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. +# +# Authors: Joep Tool + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') + pkg_gazebo_ros = get_package_share_directory('gazebo_ros') + + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + x_pose = LaunchConfiguration('x_pose', default='-1.0') + y_pose = LaunchConfiguration('y_pose', default='-5.0') + + world = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'worlds', + 'my_ghar.world' + ) + + gzserver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') + ), + launch_arguments={'world': world}.items() + ) + + gzclient_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') + ) + ) + + robot_state_publisher_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') + ), + launch_arguments={'use_sim_time': use_sim_time}.items() + ) + + spawn_turtlebot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py') + ), + launch_arguments={ + 'x_pose': x_pose, + 'y_pose': y_pose + }.items() + ) + + ld = LaunchDescription() + + # Add the commands to the launch description + ld.add_action(gzserver_cmd) + ld.add_action(gzclient_cmd) + ld.add_action(robot_state_publisher_cmd) + ld.add_action(spawn_turtlebot_cmd) + + return ld diff --git a/turtlebot3_gazebo/worlds/my_world.world b/turtlebot3_gazebo/worlds/my_world.world new file mode 100644 index 00000000..889ec626 --- /dev/null +++ b/turtlebot3_gazebo/worlds/my_world.world @@ -0,0 +1,2071 @@ + + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + -0.177857 -1.27642 0 0 -0 0 + + + 6.72319 4.26822 0.083333 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 4.26822 0.083333 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 4.03488 0.25 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 4.03488 0.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 3.80155 0.416667 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 3.80155 0.416667 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 3.56822 0.583333 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 3.56822 0.583333 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 3.33488 0.75 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 3.33488 0.75 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 3.10155 0.916667 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 3.10155 0.916667 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 2.86822 1.08333 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 2.86822 1.08333 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 2.63488 1.25 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 2.63488 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 2.40155 1.41667 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 2.40155 1.41667 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 2.16822 1.58333 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 2.16822 1.58333 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 1.93488 1.75 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 1.93488 1.75 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 1.70155 1.91667 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 1.70155 1.91667 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 1.46822 2.08333 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 1.46822 2.08333 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 1.23488 2.25 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 1.23488 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 6.72319 1.00155 2.41667 0 -0 0 + + + 1 0.233333 0.166667 + + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.233333 0.166667 + + + 6.72319 1.00155 2.41667 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 5 0.15 2.5 + + + + + 0.435294 0.796078 0.67451 1 + + + 0 + + + -7.3 -2.00362 0 0 -0 -1.5708 + 0 + 0 + 0 + + + 2.42077 1.74992 0 0 -0 -1.5708 + + -2.18898 0 1.25 0 -0 0 + + + 1.12204 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + + + + 1.12204 0.15 2.5 + + + -2.18898 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.561021 0 0.25 0 -0 0 + + + 4.37796 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + + + + + 4.37796 0.15 0.5 + + + 0.561021 0 0.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.961021 0 1.5 0 -0 0 + + + 3.57796 0.15 2 + + + + + 1 1 1 1 + + + 0 + + + + + + 3.57796 0.15 2 + + + 0.961021 0 1.5 0 -0 0 + 10 + + + + + + + + + + + + + + + -1.22796 0 1.9 0 -0 0 + + + 0.8 0.15 1.2 + + + + + 1 1 1 1 + + + 0 + + + + + + 0.8 0.15 1.2 + + + -1.22796 0 1.9 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + -0.254226 -0.920709 0 0 -0 3.14159 + + -1.67197 0 1.25 0 -0 0 + + + 2.15606 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + + + + 2.15606 0.15 2.5 + + + -1.67197 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.52803 0 1.25 0 -0 0 + + + 2.44394 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + + + + 2.44394 0.15 2.5 + + + 1.52803 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + -0.143938 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + + + + + 0.9 0.15 0.5 + + + -0.143938 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + + + + + 14.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 14.75 0.15 2.5 + + + + + 0.435294 0.796078 0.67451 1 + + + 0 + + + -0 -4.42862 0 0 -0 0 + 0 + 0 + 0 + + + + + + 9 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 9 0.15 2.5 + + + + + 0.435294 0.796078 0.67451 1 + + + 0 + + + 7.3 -0.003619 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 14.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 14.75 0.15 2.5 + + + + + 0.435294 0.796078 0.67451 1 + + + 0 + + + -0 4.42138 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 4.15 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.15 0.15 2.5 + + + + + 0.435294 0.796078 0.67451 1 + + + 0 + + + -7.3 2.42138 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 5.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 5.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -2.93358 1.75362 0 0 -0 1.5708 + 0 + 0 + 0 + + + -5.10858 -0.921381 0 0 -0 3.14159 + + -1.30309 0 1.25 0 -0 0 + + + 1.89383 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + + + + 1.89383 0.15 2.5 + + + -1.30309 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 1.39691 0 1.25 0 -0 0 + + + 1.70617 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + + + + 1.70617 0.15 2.5 + + + 1.39691 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0.093831 0 2.25 0 -0 0 + + + 0.9 0.15 0.5 + + + + + 1 1 1 1 + + + 0 + + + + + + 0.9 0.15 0.5 + + + 0.093831 0 2.25 0 -0 0 + 10 + + + + + + + + + + + + + + 0 + 0 + 0 + + 1 + + + 1 + + + 1 + + + 0 0.005 0.6 0 -0 0 + + + 0.9 0.01 1.2 + + + 10 + + + + + + + + + + + + + + + 0 0.005 0.6 0 -0 0 + + + 0.9 0.01 1.2 + + + + + + + + 0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + 10 + + + + + + + + + + + + + + + 0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + + + + + + -0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + 10 + + + + + + + + + + + + + + + -0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + + + + + + 0 -0.195 0.03 0 -0 0 + + + 0.88 0.4 0.06 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 0.03 0 -0 0 + + + 0.88 0.4 0.06 + + + + + + + + 0 -0.195 1.19 0 -0 0 + + + 0.88 0.4 0.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 1.19 0 -0 0 + + + 0.88 0.4 0.02 + + + + + + + + 0 -0.195 0.43 0 -0 0 + + + 0.88 0.4 0.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 0.43 0 -0 0 + + + 0.88 0.4 0.02 + + + + + + + + 0 -0.195 0.8 0 -0 0 + + + 0.88 0.4 0.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 0.8 0 -0 0 + + + 0.88 0.4 0.02 + + + + + + + 0 + 0 + 0 + + 1.47126 3.04335 0 0 -0 0 + + + 1 + + + 0 0 0.755 0 -0 0 + + + 0.913 0.913 0.04 + + + 10 + + + + + + + + + + + + + + + 0 0 0.37 0 -0 0 + + + 0.042 0.042 0.74 + + + 10 + + + + + + + + + + + + + + + 0 0 0.02 0 -0 0 + + + 0.56 0.56 0.04 + + + 10 + + + + + + + + + + + + + + + + + model://cafe_table/meshes/cafe_table.dae + + + + 0 + 0 + 0 + + 4.31597 -3.19187 0 0 -0 0 + + + -1.28597 -2.81719 0.15 0 -0 0 + + + 2 + + 0.0416667 + 0 + 0 + 0.0566667 + 0 + 0.0683333 + + 0 0 0 0 -0 0 + + + + + 0.5 0.4 0.3 + + + + + + 1 + 1 + + + + + + + + 1e+07 + 1 + 0.001 + 0.1 + + + + + 10 + + + 0 0 -0.15 0 -0 0 + + + model://cardboard_box/meshes/cardboard_box.dae + 1.25932 1.00745 0.755591 + + + + 0 + 0 + 0 + + + + + + + + 1.5 1.5 1.5 + model://dumpster/meshes/dumpster.dae + + + 10 + + + + + + + + + + + + + + + + + 1.5 1.5 1.5 + model://dumpster/meshes/dumpster.dae + + + + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + 4.43895 -1.31443 0 0 -0 0 + + + + + + + model://mailbox/meshes/mailbox.dae + + + 10 + + + + + + + + + + + + + + + + + model://mailbox/meshes/mailbox.dae + + + + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + -7.67239 -6.57222 0 0 -0 0 + + + + 0 0 0 0 -0 0 + + 20 + + 2.43 + 0 + 0 + 1.1 + 0 + 3.47 + + 0 0 0 0 -0 0 + + + 0 0 0.05 0 -0 0 + + + model://euro_pallet/meshes/pallet.dae + 0.1 0.1 0.1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.05 0 -0 0 + + + model://euro_pallet/meshes/pallet.dae + 0.1 0.1 0.1 + + + + 0 + 0 + 0 + + -6.79932 -2.97739 0 0 -0 0 + + + 395 748000000 + 431 190528785 + 1732375511 935559939 + 395748 + + 4.43895 -1.31443 0.001376 2e-06 -0 0 + 1 1 1 + + 4.43895 -1.31443 0.001376 2e-06 -0 0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + -7.67239 -6.57222 -9e-06 -8e-06 -5e-06 0 + 1 1 1 + + -7.67239 -6.57222 -9e-06 -8e-06 -5e-06 0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 1.47126 3.04335 0 0 -0 0 + 1 1 1 + + 1.47126 3.04335 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 4.31597 -3.19187 0 0 -0 0 + 1 1 1 + + 4.31597 -3.19187 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -1.28597 -2.81719 0.149 0 -0 0 + 1 1 1 + + -1.28597 -2.81719 0.149 0 -0 0 + 0 0 0 0 -0 0 + -0 1.9e-05 -0 -0.000128 -2e-06 0 + -1e-06 3.8e-05 -1e-06 0 -0 0 + + + + -6.80465 -2.97324 0.072 0 0 -0.013016 + 1 1 1 + + -6.80465 -2.97324 0.072 0 0 -0.013016 + 0 0 0 0 -0 0 + -2.57413 -0.069182 10.6731 -2.43541 1.17473 3.14157 + -51.4827 -1.38364 213.463 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.177857 -1.27642 0 0 -0 0 + 1 1 1 + + -0.177857 -1.27642 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.47786 -3.28004 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 2.24291 0.4735 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.432083 -2.19713 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.177857 -5.70504 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 7.12214 -1.28004 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.177857 3.14496 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.47786 1.14496 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -3.11144 0.4772 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.28644 -2.1978 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 5.83068 -18.8989 23.4054 -0 0.883643 1.81219 + orbit + perspective + + + +