nav2 file added

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()

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