Added Loopback Simulator Package

nav2_loopback_sim/.amentcpplint

@@ -0,0 +1,4 @@
+filter=-build/c++14
+extensions=h,hpp,c,cpp
+linelength=120
+

nav2_loopback_sim/.uncrustify.cfg

@@ -0,0 +1,11 @@
+--no-backup
+--replace
+--no-backup-if-no-changes
+--indent_with_tabs
+--tabs=4
+--utf8
+--lf
+--preserve-date
+--align-pointer=name
+--align-reference=name
+

nav2_loopback_sim/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_loopback_sim)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+include_directories(
+  include
+)
+
+set(executable_name sim_node)
+set(library_name ${executable_name}_core)
+
+set(dependencies
+  rclcpp
+  std_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+)
+
+add_library(${library_name} SHARED
+  src/loopback_simulator.cpp
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+add_executable(${executable_name}
+  src/main.cpp
+)
+
+target_link_libraries(${executable_name} ${library_name}) 
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+rclcpp_components_register_nodes(${library_name} "nav2_loopback_sim::SimServer")
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  #add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+ament_package()

nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp

@@ -0,0 +1,93 @@
+// Copyright (c) 2019 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_
+#define NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_
+
+#pragma once
+#include <iostream>
+#include <functional>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2_ros/static_transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/convert.h"
+#include "tf2_ros/buffer.h"
+#include "tf2/exceptions.h"
+#include "tf2/transform_datatypes.h"
+
+namespace nav2_loopback_sim
+{
+/**
+ * @class nav2_planner::LoopbackSimulator
+ * @brief A class that is a lightweight simulation alternative designed to facilitate
+ * testing of higher-level behavioral attributes
+*/
+class LoopbackSimulator : public rclcpp::Node
+{
+public:
+  /**
+  * @brief A constructor for nav2_planner::LoopbackSimulator
+  */
+  LoopbackSimulator();
+
+private:
+  /**
+  * @brief Called when velocity commands are received
+  * @param msg twist velocity message
+  */
+  void twistCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+
+  /**
+  * @brief Called when initial pose is set by the user
+  * @param msg initial pose
+  */
+  void initposeCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+
+  /**
+  * @brief Timer function to continously broadcast map->odom tf
+  */
+  void timerCallback();
+
+  // Poses
+  geometry_msgs::msg::PoseWithCovarianceStamped init_pose_;  // init odom pose wrt map frame
+  geometry_msgs::msg::PoseWithCovarianceStamped odom_updated_pose_;
+  // Transformed init_pose_ from "map" to "odom" frame
+  bool init_pose_set_ = false;
+  bool init_odom_base_published_ = false;
+  bool transform_initpose_once_ = true;
+
+  // Subscribers
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr vel_subscriber_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    init_pose_subscriber_;
+
+  // tf
+  std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf_broadcaster_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+}  // namespace nav2_loopback_sim
+
+#endif  // NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_

nav2_loopback_sim/launch/loopback_sim_launch.py

@@ -0,0 +1,203 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, 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
+from nav2_common.launch import RewrittenYaml
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+
+    # Create the launch configuration variables
+    namespace = LaunchConfiguration('namespace')
+    #use_namespace = LaunchConfiguration('use_namespace')
+    map_yaml_file = LaunchConfiguration('map')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    params_file = LaunchConfiguration('params_file')
+    autostart = LaunchConfiguration('autostart')
+
+    # Launch configuration variables specific to simulation
+    rviz_config_file = LaunchConfiguration('rviz_config_file')
+    use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
+    use_rviz = LaunchConfiguration('use_rviz')
+    #headless = LaunchConfiguration('headless')
+    #world = LaunchConfiguration('world')
+
+    # Map fully qualified names to relative ones so the node's namespace can be prepended.
+    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+    # TODO(orduno) Substitute with `PushNodeRemapping`
+    #              https://github.com/ros2/launch_ros/issues/56
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
+
+    param_substitutions = {
+        'use_sim_time': use_sim_time,
+        'yaml_filename': map_yaml_file}
+
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key=namespace,
+        param_rewrites=param_substitutions,
+        convert_types=True)
+
+    lifecycle_nodes = ['map_server']
+
+    # Declare the launch arguments
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
+    declare_use_namespace_cmd = DeclareLaunchArgument(
+        'use_namespace',
+        default_value='false',
+        description='Whether to apply a namespace to the navigation stack')
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        'map',
+        default_value=os.path.join(
+            bringup_dir, 'maps', 'turtlebot3_world.yaml'),
+        description='Full path to map file to load')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+       'use_sim_time',
+       default_value='true',
+       description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='true',
+        description='Automatically startup the nav2 stack')
+
+    declare_rviz_config_file_cmd = DeclareLaunchArgument(
+        'rviz_config_file',
+        default_value=os.path.join(
+            bringup_dir, 'rviz', 'nav2_default_view.rviz'),
+        description='Full path to the RVIZ config file to use')
+
+    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
+        'use_robot_state_pub',
+        default_value='True',
+        description='Whether to start the robot state publisher')
+
+    declare_use_rviz_cmd = DeclareLaunchArgument(
+        'use_rviz',
+        default_value='True',
+        description='Whether to start RVIZ')
+
+    declare_world_cmd = DeclareLaunchArgument(
+        'world',
+        # TODO(orduno) Switch back once ROS argument passing has been fixed upstream
+        #              https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
+        # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
+        # worlds/turtlebot3_worlds/waffle.model')
+        default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'),
+        description='Full path to world model file to load')
+
+    urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
+
+    start_robot_state_publisher_cmd = Node(
+        condition=IfCondition(use_robot_state_pub),
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        namespace=namespace,
+        output='screen',
+        parameters=[{'use_sim_time': use_sim_time}],
+        remappings=remappings,
+        arguments=[urdf])
+
+    rviz_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_dir, 'rviz_launch.py')),
+        condition=IfCondition(use_rviz),
+        launch_arguments={'namespace': '',
+                          'use_namespace': 'False',
+                          'rviz_config': rviz_config_file}.items())
+
+
+    nav_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
+            launch_arguments={'namespace': namespace,
+                              'use_sim_time': use_sim_time,
+                              'autostart': autostart,
+                              'params_file': params_file,
+                              'use_lifecycle_mgr': 'false',
+                              'map_subscribe_transient_local': 'true'}.items())
+
+    loopback_sim_node = Node(
+                        package='nav2_loopback_sim',
+                        executable='sim_node',
+                        name='loopback_simulator_node',
+                        output='screen',
+                        parameters=[{'timer_frequency': 1.0}])
+
+    map_server_cmd = Node(
+                    package='nav2_map_server',
+                    executable='map_server',
+                    name='map_server',
+                    output='screen',
+                    parameters=[configured_params],
+                    remappings=remappings)
+
+    lifecycle_cmd = Node(
+                    package='nav2_lifecycle_manager',
+                    executable='lifecycle_manager',
+                    name='lifecycle_manager_localization',
+                    output='screen',
+                    parameters=[{'use_sim_time': use_sim_time},
+                                {'autostart': autostart},
+                                {'node_names': lifecycle_nodes}])
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_namespace_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+
+    ld.add_action(declare_rviz_config_file_cmd)
+    ld.add_action(declare_use_robot_state_pub_cmd)
+    ld.add_action(declare_use_rviz_cmd)
+    ld.add_action(declare_world_cmd)
+    ld.add_action(nav_cmd)
+    ld.add_action(loopback_sim_node)
+
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(start_robot_state_publisher_cmd)
+    ld.add_action(rviz_cmd)
+    ld.add_action(map_server_cmd)
+    ld.add_action(lifecycle_cmd)
+
+    return ld