From 0218b7b909197263c8b00a66f13fd961aa32356b Mon Sep 17 00:00:00 2001 From: smitdumore Date: Mon, 11 Dec 2023 15:50:32 -0500 Subject: [PATCH 01/12] Added Loopback Simulator Package --- nav2_loopback_sim/.amentcpplint | 4 + nav2_loopback_sim/.uncrustify.cfg | 11 + nav2_loopback_sim/CMakeLists.txt | 78 +++++++ .../nav2_loopback_sim/loopback_simulator.hpp | 93 ++++++++ .../launch/loopback_sim_launch.py | 203 ++++++++++++++++++ nav2_loopback_sim/package.xml | 30 +++ nav2_loopback_sim/readme.md | 7 + nav2_loopback_sim/src/loopback_simulator.cpp | 164 ++++++++++++++ nav2_loopback_sim/src/main.cpp | 26 +++ navigation2/package.xml | 1 + 10 files changed, 617 insertions(+) create mode 100644 nav2_loopback_sim/.amentcpplint create mode 100644 nav2_loopback_sim/.uncrustify.cfg create mode 100644 nav2_loopback_sim/CMakeLists.txt create mode 100644 nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp create mode 100644 nav2_loopback_sim/launch/loopback_sim_launch.py create mode 100644 nav2_loopback_sim/package.xml create mode 100644 nav2_loopback_sim/readme.md create mode 100644 nav2_loopback_sim/src/loopback_simulator.cpp create mode 100644 nav2_loopback_sim/src/main.cpp diff --git a/nav2_loopback_sim/.amentcpplint b/nav2_loopback_sim/.amentcpplint new file mode 100644 index 0000000000..8471a9e8d6 --- /dev/null +++ b/nav2_loopback_sim/.amentcpplint @@ -0,0 +1,4 @@ +filter=-build/c++14 +extensions=h,hpp,c,cpp +linelength=120 + diff --git a/nav2_loopback_sim/.uncrustify.cfg b/nav2_loopback_sim/.uncrustify.cfg new file mode 100644 index 0000000000..a97b8c2330 --- /dev/null +++ b/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 + diff --git a/nav2_loopback_sim/CMakeLists.txt b/nav2_loopback_sim/CMakeLists.txt new file mode 100644 index 0000000000..9f218aaca3 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp new file mode 100644 index 0000000000..a35b9a6d4d --- /dev/null +++ b/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 +#include +#include + +#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::SharedPtr vel_subscriber_; + rclcpp::Subscription::SharedPtr + init_pose_subscriber_; + + // tf + std::unique_ptr tf_broadcaster_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace nav2_loopback_sim + +#endif // NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_ diff --git a/nav2_loopback_sim/launch/loopback_sim_launch.py b/nav2_loopback_sim/launch/loopback_sim_launch.py new file mode 100644 index 0000000000..5fe132f136 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml new file mode 100644 index 0000000000..253cba5218 --- /dev/null +++ b/nav2_loopback_sim/package.xml @@ -0,0 +1,30 @@ + + + + nav2_loopback_sim + 1.0.0 + This ROS package offers a simplified Gazebo replacement, + focusing on sending robot velocity commands to TF as odometry, + facilitating high-level behavioral testing without the complexity of full simulation + Smit Dumore + Apache-2.0 + + ament_cmake + + launch + + rclcpp + geometry_msgs + std_msgs + tf2_msgs + tf2_ros + tf2_ros_msgs + tf2_geometry_msgs + + ament_lint_common + ament_lint_auto + + + ament_cmake + + diff --git a/nav2_loopback_sim/readme.md b/nav2_loopback_sim/readme.md new file mode 100644 index 0000000000..6ddd6eca8d --- /dev/null +++ b/nav2_loopback_sim/readme.md @@ -0,0 +1,7 @@ +# Nav2 Loopback Simulator + +The Nav2 Loopback Simulator package offers a simplified Gazebo replacement, focusing on sending robot velocity commands to TF as odometry, facilitating high-level behavioral testing without the complexity of full simulation. + +## Usage + +`ros2 launch nav2_loopback_sim loopback_sim_launch.py map:=/path_to_ros_ws/navigation2/nav2_bringup/bringup/maps/turtlebot3_world.yaml` \ No newline at end of file diff --git a/nav2_loopback_sim/src/loopback_simulator.cpp b/nav2_loopback_sim/src/loopback_simulator.cpp new file mode 100644 index 0000000000..2d8f710c20 --- /dev/null +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -0,0 +1,164 @@ +// Copyright (c) 2018 Intel Corporation +// 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. + +#include +#include "nav2_loopback_sim/loopback_simulator.hpp" + +namespace nav2_loopback_sim +{ + +LoopbackSimulator::LoopbackSimulator() +: Node("loopback_simulator_node") +{ + // Initialise subscribers + vel_subscriber_ = this->create_subscription( + "cmd_vel", 10, std::bind(&LoopbackSimulator::twistCallback, this, std::placeholders::_1)); + + init_pose_subscriber_ = create_subscription( + "initialpose", 10, + std::bind(&LoopbackSimulator::initposeCallback, this, std::placeholders::_1)); + + // Frequency parameter for timer function + double timer_frequency{0.0}; + declare_parameter("timer_frequency", 10.0); + get_parameter("timer_frequency", timer_frequency); + + // Create a timer + timer_ = create_wall_timer( + std::chrono::duration(1.0 / timer_frequency), + std::bind(&LoopbackSimulator::timerCallback, this)); + + // Initialize tf broadcaster + tf_broadcaster_ = std::make_unique(*this); + + // tf + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +void LoopbackSimulator::twistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + if (init_pose_set_ == false) {return;} + + double dt = 0.1; // Adjust the time step + + // Transform initpose from map to odom frame + if (transform_initpose_once_ == true) { + try { + geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( + "odom", + init_pose_.header.frame_id, + tf2::TimePointZero); + + tf2::doTransform(init_pose_, odom_updated_pose_, transform); + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("logger_name"), + "Could not transform init pose from Map to Odom."); + } + + transform_initpose_once_ = false; + } + + // Update the transformed pose in the local variable + odom_updated_pose_.header.frame_id = "odom"; + odom_updated_pose_.header.stamp = this->now(); + + tf2::Quaternion base_link_orientation( + odom_updated_pose_.pose.pose.orientation.x, + odom_updated_pose_.pose.pose.orientation.y, + odom_updated_pose_.pose.pose.orientation.z, + odom_updated_pose_.pose.pose.orientation.w + ); + + tf2::Quaternion angular_change; + angular_change.setRPY(msg->angular.x * dt, msg->angular.y * dt, msg->angular.z * dt); + base_link_orientation *= angular_change; + + odom_updated_pose_.pose.pose.orientation.x = base_link_orientation.x(); + odom_updated_pose_.pose.pose.orientation.y = base_link_orientation.y(); + odom_updated_pose_.pose.pose.orientation.z = base_link_orientation.z(); + odom_updated_pose_.pose.pose.orientation.w = base_link_orientation.w(); + + // Forward integrate robot position and orientation using velocity commands + tf2::Matrix3x3 rotation_matrix(base_link_orientation); + tf2::Vector3 linear_velocity(msg->linear.x, msg->linear.y, msg->linear.z); + tf2::Vector3 rotated_linear_velocity = rotation_matrix * linear_velocity; + + odom_updated_pose_.pose.pose.position.x += rotated_linear_velocity.x() * dt; + odom_updated_pose_.pose.pose.position.y += rotated_linear_velocity.y() * dt; + odom_updated_pose_.pose.pose.position.z += rotated_linear_velocity.z() * dt; + + // Broadcast "odom" to "base_link" transform using the local variable + geometry_msgs::msg::TransformStamped odom_to_base_transform; + odom_to_base_transform.header.stamp = odom_updated_pose_.header.stamp; + odom_to_base_transform.header.frame_id = "odom"; + odom_to_base_transform.child_frame_id = "base_link"; + odom_to_base_transform.transform.translation.x = odom_updated_pose_.pose.pose.position.x; + odom_to_base_transform.transform.translation.y = odom_updated_pose_.pose.pose.position.y; + odom_to_base_transform.transform.translation.z = odom_updated_pose_.pose.pose.position.z; + odom_to_base_transform.transform.rotation = odom_updated_pose_.pose.pose.orientation; + + tf_broadcaster_->sendTransform(odom_to_base_transform); +} + +void LoopbackSimulator::initposeCallback( + const + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + RCLCPP_INFO(this->get_logger(), "Received initial pose wrt map"); + init_pose_ = *msg; + init_pose_set_ = true; + + if (init_odom_base_published_ == false) { + // Publish Identity transform between odom to base_footprint + geometry_msgs::msg::TransformStamped odom_to_base_transform; + odom_to_base_transform.header.stamp = this->now(); + odom_to_base_transform.header.frame_id = "odom"; + odom_to_base_transform.child_frame_id = "base_footprint"; + odom_to_base_transform.transform.translation.x = 0.0; + odom_to_base_transform.transform.translation.y = 0.0; + odom_to_base_transform.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0); + odom_to_base_transform.transform.rotation.x = q.x(); + odom_to_base_transform.transform.rotation.y = q.y(); + odom_to_base_transform.transform.rotation.z = q.z(); + odom_to_base_transform.transform.rotation.w = q.w(); + + tf_broadcaster_->sendTransform(odom_to_base_transform); + init_odom_base_published_ = true; + } +} + +void LoopbackSimulator::timerCallback() +{ + if (init_pose_set_ == false) {return;} + // RCLCPP_INFO(this->get_logger(), "Publishing map->odom tf"); + + // Publish map to odom transform + geometry_msgs::msg::TransformStamped map_to_odom_transform; + map_to_odom_transform.header.stamp = this->now(); + map_to_odom_transform.header.frame_id = "map"; + map_to_odom_transform.child_frame_id = "odom"; + map_to_odom_transform.transform.translation.x = init_pose_.pose.pose.position.x; + map_to_odom_transform.transform.translation.y = init_pose_.pose.pose.position.y; + map_to_odom_transform.transform.translation.z = init_pose_.pose.pose.position.z; + map_to_odom_transform.transform.rotation = init_pose_.pose.pose.orientation; + + tf_broadcaster_->sendTransform(map_to_odom_transform); +} + +} // namespace nav2_loopback_sim diff --git a/nav2_loopback_sim/src/main.cpp b/nav2_loopback_sim/src/main.cpp new file mode 100644 index 0000000000..fc30faedd5 --- /dev/null +++ b/nav2_loopback_sim/src/main.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2018 Intel Corporation +// 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. + +#include +#include "nav2_loopback_sim/loopback_simulator.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/navigation2/package.xml b/navigation2/package.xml index 0769a1525e..107f94fb8b 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -41,6 +41,7 @@ nav2_velocity_smoother nav2_voxel_grid nav2_waypoint_follower + nav2_loopback_sim ament_cmake From eaa483acfac7be9278b238c8f8a0a8da59f18275 Mon Sep 17 00:00:00 2001 From: smitdumore Date: Wed, 13 Dec 2023 11:37:55 -0500 Subject: [PATCH 02/12] fixed build farm issues --- .../include/nav2_loopback_sim/loopback_simulator.hpp | 2 +- nav2_loopback_sim/package.xml | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp index a35b9a6d4d..df75afd749 100644 --- a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp +++ b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp @@ -28,9 +28,9 @@ #include "tf2_ros/static_transform_broadcaster.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.h" #include "tf2/LinearMath/Quaternion.h" #include "tf2/convert.h" -#include "tf2_ros/buffer.h" #include "tf2/exceptions.h" #include "tf2/transform_datatypes.h" diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index 253cba5218..311c80d988 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -16,9 +16,8 @@ rclcpp geometry_msgs std_msgs - tf2_msgs + tf2 tf2_ros - tf2_ros_msgs tf2_geometry_msgs ament_lint_common From 8fd44d7d79addd4e45577f4fbdd2cfd091f41607 Mon Sep 17 00:00:00 2001 From: smitdumore Date: Wed, 13 Dec 2023 11:41:38 -0500 Subject: [PATCH 03/12] updated project version --- nav2_loopback_sim/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index 311c80d988..07099ecb28 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -2,7 +2,7 @@ nav2_loopback_sim - 1.0.0 + 1.2.0 This ROS package offers a simplified Gazebo replacement, focusing on sending robot velocity commands to TF as odometry, facilitating high-level behavioral testing without the complexity of full simulation From 9292122d9cbe712f8ec0454fe2230f470278ae7f Mon Sep 17 00:00:00 2001 From: smitdumore Date: Wed, 13 Dec 2023 11:42:54 -0500 Subject: [PATCH 04/12] removed unwanted lint and uncrustify files --- nav2_loopback_sim/.amentcpplint | 4 ---- nav2_loopback_sim/.uncrustify.cfg | 11 ----------- 2 files changed, 15 deletions(-) delete mode 100644 nav2_loopback_sim/.amentcpplint delete mode 100644 nav2_loopback_sim/.uncrustify.cfg diff --git a/nav2_loopback_sim/.amentcpplint b/nav2_loopback_sim/.amentcpplint deleted file mode 100644 index 8471a9e8d6..0000000000 --- a/nav2_loopback_sim/.amentcpplint +++ /dev/null @@ -1,4 +0,0 @@ -filter=-build/c++14 -extensions=h,hpp,c,cpp -linelength=120 - diff --git a/nav2_loopback_sim/.uncrustify.cfg b/nav2_loopback_sim/.uncrustify.cfg deleted file mode 100644 index a97b8c2330..0000000000 --- a/nav2_loopback_sim/.uncrustify.cfg +++ /dev/null @@ -1,11 +0,0 @@ ---no-backup ---replace ---no-backup-if-no-changes ---indent_with_tabs ---tabs=4 ---utf8 ---lf ---preserve-date ---align-pointer=name ---align-reference=name - From b5f31b184c7467ad0bf968788839f290bca6957e Mon Sep 17 00:00:00 2001 From: smitdumore Date: Wed, 13 Dec 2023 12:13:47 -0500 Subject: [PATCH 05/12] added component regestration --- nav2_loopback_sim/CMakeLists.txt | 4 +++- .../nav2_loopback_sim/loopback_simulator.hpp | 8 +++++++- nav2_loopback_sim/src/loopback_simulator.cpp | 17 +++++++++++++++-- 3 files changed, 25 insertions(+), 4 deletions(-) diff --git a/nav2_loopback_sim/CMakeLists.txt b/nav2_loopback_sim/CMakeLists.txt index 9f218aaca3..b43de685e6 100644 --- a/nav2_loopback_sim/CMakeLists.txt +++ b/nav2_loopback_sim/CMakeLists.txt @@ -4,6 +4,7 @@ project(nav2_loopback_sim) # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) @@ -19,6 +20,7 @@ set(library_name ${executable_name}_core) set(dependencies rclcpp + rclcpp_components std_msgs geometry_msgs tf2 @@ -44,7 +46,7 @@ ament_target_dependencies(${executable_name} ${dependencies} ) -rclcpp_components_register_nodes(${library_name} "nav2_loopback_sim::SimServer") +rclcpp_components_register_nodes(${library_name} "nav2_loopback_sim::LoopbackSimulator") install(TARGETS ${library_name} ARCHIVE DESTINATION lib diff --git a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp index df75afd749..561c81fa37 100644 --- a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp +++ b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp @@ -47,7 +47,13 @@ class LoopbackSimulator : public rclcpp::Node /** * @brief A constructor for nav2_planner::LoopbackSimulator */ - LoopbackSimulator(); + explicit LoopbackSimulator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** + * @brief A destructor for nav2_loopback_sim::LoopbackSimulator + * + */ + ~LoopbackSimulator(); + private: /** diff --git a/nav2_loopback_sim/src/loopback_simulator.cpp b/nav2_loopback_sim/src/loopback_simulator.cpp index 2d8f710c20..69ffa735ab 100644 --- a/nav2_loopback_sim/src/loopback_simulator.cpp +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -19,8 +19,8 @@ namespace nav2_loopback_sim { -LoopbackSimulator::LoopbackSimulator() -: Node("loopback_simulator_node") +LoopbackSimulator::LoopbackSimulator(const rclcpp::NodeOptions & options) +: Node("loopback_simulator_node", options) { // Initialise subscribers vel_subscriber_ = this->create_subscription( @@ -48,6 +48,12 @@ LoopbackSimulator::LoopbackSimulator() tf_listener_ = std::make_shared(*tf_buffer_); } +LoopbackSimulator::~LoopbackSimulator() +{ + RCLCPP_INFO(get_logger(), "Destroying %s", get_name()); +} + + void LoopbackSimulator::twistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { if (init_pose_set_ == false) {return;} @@ -162,3 +168,10 @@ void LoopbackSimulator::timerCallback() } } // namespace nav2_loopback_sim + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_loopback_sim::LoopbackSimulator) From 7e4eda2ed8404ae382e8bba6d9f798e6777c0c8a Mon Sep 17 00:00:00 2001 From: smitdumore Date: Wed, 13 Dec 2023 12:17:35 -0500 Subject: [PATCH 06/12] updated copyright --- .../include/nav2_loopback_sim/loopback_simulator.hpp | 2 +- nav2_loopback_sim/launch/loopback_sim_launch.py | 2 +- nav2_loopback_sim/src/loopback_simulator.cpp | 3 +-- nav2_loopback_sim/src/main.cpp | 3 +-- 4 files changed, 4 insertions(+), 6 deletions(-) diff --git a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp index 561c81fa37..c6c1c63600 100644 --- a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp +++ b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Samsung Research America +// Copyright (c) 2023 Smit Dumore // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_loopback_sim/launch/loopback_sim_launch.py b/nav2_loopback_sim/launch/loopback_sim_launch.py index 5fe132f136..36aa0b9cd1 100644 --- a/nav2_loopback_sim/launch/loopback_sim_launch.py +++ b/nav2_loopback_sim/launch/loopback_sim_launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2023 Smit Dumore # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/nav2_loopback_sim/src/loopback_simulator.cpp b/nav2_loopback_sim/src/loopback_simulator.cpp index 69ffa735ab..1d59f69a60 100644 --- a/nav2_loopback_sim/src/loopback_simulator.cpp +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -1,5 +1,4 @@ -// Copyright (c) 2018 Intel Corporation -// Copyright (c) 2019 Samsung Research America +// Copyright (c) 2023 Smit Dumore // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_loopback_sim/src/main.cpp b/nav2_loopback_sim/src/main.cpp index fc30faedd5..8db99d2635 100644 --- a/nav2_loopback_sim/src/main.cpp +++ b/nav2_loopback_sim/src/main.cpp @@ -1,5 +1,4 @@ -// Copyright (c) 2018 Intel Corporation -// Copyright (c) 2019 Samsung Research America +// Copyright (c) 2023 Smit Dumore // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From d329e87ce81452ea4cd644566dd641ed19d7a288 Mon Sep 17 00:00:00 2001 From: smitdumore Date: Wed, 13 Dec 2023 12:24:51 -0500 Subject: [PATCH 07/12] fixed typos and minor mistakes --- .../include/nav2_loopback_sim/loopback_simulator.hpp | 7 +++---- nav2_loopback_sim/src/loopback_simulator.cpp | 12 +++++------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp index c6c1c63600..c8bc63c84a 100644 --- a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp +++ b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp @@ -15,7 +15,6 @@ #ifndef NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_ #define NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_ -#pragma once #include #include #include @@ -37,7 +36,7 @@ namespace nav2_loopback_sim { /** - * @class nav2_planner::LoopbackSimulator + * @class nav2_loopback_sim::LoopbackSimulator * @brief A class that is a lightweight simulation alternative designed to facilitate * testing of higher-level behavioral attributes */ @@ -45,7 +44,7 @@ class LoopbackSimulator : public rclcpp::Node { public: /** - * @brief A constructor for nav2_planner::LoopbackSimulator + * @brief A constructor for nav2_loopback_sim::LoopbackSimulator */ explicit LoopbackSimulator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** @@ -66,7 +65,7 @@ class LoopbackSimulator : public rclcpp::Node * @brief Called when initial pose is set by the user * @param msg initial pose */ - void initposeCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void initPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** * @brief Timer function to continously broadcast map->odom tf diff --git a/nav2_loopback_sim/src/loopback_simulator.cpp b/nav2_loopback_sim/src/loopback_simulator.cpp index 1d59f69a60..1a4cadca02 100644 --- a/nav2_loopback_sim/src/loopback_simulator.cpp +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -21,13 +21,13 @@ namespace nav2_loopback_sim LoopbackSimulator::LoopbackSimulator(const rclcpp::NodeOptions & options) : Node("loopback_simulator_node", options) { - // Initialise subscribers + // Initialize subscribers vel_subscriber_ = this->create_subscription( "cmd_vel", 10, std::bind(&LoopbackSimulator::twistCallback, this, std::placeholders::_1)); init_pose_subscriber_ = create_subscription( "initialpose", 10, - std::bind(&LoopbackSimulator::initposeCallback, this, std::placeholders::_1)); + std::bind(&LoopbackSimulator::initPoseCallback, this, std::placeholders::_1)); // Frequency parameter for timer function double timer_frequency{0.0}; @@ -39,10 +39,8 @@ LoopbackSimulator::LoopbackSimulator(const rclcpp::NodeOptions & options) std::chrono::duration(1.0 / timer_frequency), std::bind(&LoopbackSimulator::timerCallback, this)); - // Initialize tf broadcaster + // Transforms tf_broadcaster_ = std::make_unique(*this); - - // tf tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); } @@ -119,11 +117,11 @@ void LoopbackSimulator::twistCallback(const geometry_msgs::msg::Twist::SharedPtr tf_broadcaster_->sendTransform(odom_to_base_transform); } -void LoopbackSimulator::initposeCallback( +void LoopbackSimulator::initPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received initial pose wrt map"); + RCLCPP_INFO(this->get_logger(), "Received initial pose!"); init_pose_ = *msg; init_pose_set_ = true; From 7d01e597a3ece1742505a8c1c6544eee37245627 Mon Sep 17 00:00:00 2001 From: smitdumore Date: Wed, 13 Dec 2023 12:30:47 -0500 Subject: [PATCH 08/12] added initial pose not set warning --- nav2_loopback_sim/src/loopback_simulator.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_loopback_sim/src/loopback_simulator.cpp b/nav2_loopback_sim/src/loopback_simulator.cpp index 1a4cadca02..1add800a1f 100644 --- a/nav2_loopback_sim/src/loopback_simulator.cpp +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -53,7 +53,11 @@ LoopbackSimulator::~LoopbackSimulator() void LoopbackSimulator::twistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { - if (init_pose_set_ == false) {return;} + if (init_pose_set_ == false) + { + RCLCPP_WARN(rclcpp::get_logger("logger_name"), "Initial pose has not been set. Ignoring incoming velocity messages."); + return; + } double dt = 0.1; // Adjust the time step From 2c6f01ac40668a2fa71c01dc6fc8d6f48addc9b5 Mon Sep 17 00:00:00 2001 From: smitdumore Date: Wed, 13 Dec 2023 12:32:55 -0500 Subject: [PATCH 09/12] updated identity tranform between base and odom --- nav2_loopback_sim/src/loopback_simulator.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/nav2_loopback_sim/src/loopback_simulator.cpp b/nav2_loopback_sim/src/loopback_simulator.cpp index 1add800a1f..64d33e0be8 100644 --- a/nav2_loopback_sim/src/loopback_simulator.cpp +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -138,12 +138,10 @@ void LoopbackSimulator::initPoseCallback( odom_to_base_transform.transform.translation.x = 0.0; odom_to_base_transform.transform.translation.y = 0.0; odom_to_base_transform.transform.translation.z = 0.0; - tf2::Quaternion q; - q.setRPY(0, 0, 0); - odom_to_base_transform.transform.rotation.x = q.x(); - odom_to_base_transform.transform.rotation.y = q.y(); - odom_to_base_transform.transform.rotation.z = q.z(); - odom_to_base_transform.transform.rotation.w = q.w(); + odom_to_base_transform.transform.rotation.x = 0.0; + odom_to_base_transform.transform.rotation.y = 0.0; + odom_to_base_transform.transform.rotation.z = 0.0; + odom_to_base_transform.transform.rotation.w = 1.0; tf_broadcaster_->sendTransform(odom_to_base_transform); init_odom_base_published_ = true; From b947f401380859d09b0dfd90d0f9551ccb0cbb7a Mon Sep 17 00:00:00 2001 From: smitdumore Date: Tue, 20 Feb 2024 17:47:27 -0500 Subject: [PATCH 10/12] new updates --- nav2_loopback_sim/CMakeLists.txt | 4 +- .../nav2_loopback_sim/loopback_simulator.hpp | 26 ++- .../launch/loopback_sim_launch.py | 93 ++++++-- nav2_loopback_sim/package.xml | 1 + nav2_loopback_sim/src/loopback_simulator.cpp | 215 +++++++++++------- nav2_loopback_sim/src/main.cpp | 2 + nav2_loopback_sim/test/CMakeLists.txt | 10 + .../test/test_loopback_simulator.cpp | 109 +++++++++ 8 files changed, 354 insertions(+), 106 deletions(-) create mode 100644 nav2_loopback_sim/test/CMakeLists.txt create mode 100644 nav2_loopback_sim/test/test_loopback_simulator.cpp diff --git a/nav2_loopback_sim/CMakeLists.txt b/nav2_loopback_sim/CMakeLists.txt index b43de685e6..850b22548b 100644 --- a/nav2_loopback_sim/CMakeLists.txt +++ b/nav2_loopback_sim/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -22,6 +23,7 @@ set(dependencies rclcpp rclcpp_components std_msgs + sensor_msgs geometry_msgs tf2 tf2_ros @@ -71,7 +73,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() - #add_subdirectory(test) + add_subdirectory(test) endif() ament_export_include_directories(include) diff --git a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp index c8bc63c84a..9e2f053049 100644 --- a/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp +++ b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp @@ -18,12 +18,14 @@ #include #include #include +#include #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_geometry_msgs/tf2_geometry_msgs.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" #include "tf2_ros/static_transform_broadcaster.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/transform_broadcaster.h" @@ -49,10 +51,9 @@ class LoopbackSimulator : public rclcpp::Node explicit LoopbackSimulator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief A destructor for nav2_loopback_sim::LoopbackSimulator - * + * */ ~LoopbackSimulator(); - private: /** @@ -72,19 +73,34 @@ class LoopbackSimulator : public rclcpp::Node */ void timerCallback(); + /** + * @brief Function to publish transform between given frames + * @param pose Transform to be published + * @param parent_frame Parent frame + * @param child_frame Child frame + */ + void publishTransform( + const geometry_msgs::msg::Pose & pose, + const std::string & parent_frame, + const std::string & child_frame); + // 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; + bool zero_odom_base_published_ = false; + double dt_{0.1}; + geometry_msgs::msg::Pose relocalization_pose_; // Subscribers rclcpp::Subscription::SharedPtr vel_subscriber_; rclcpp::Subscription::SharedPtr init_pose_subscriber_; + // Publishers + rclcpp::Publisher::SharedPtr scan_publisher_; + // tf std::unique_ptr tf_broadcaster_; std::shared_ptr tf_buffer_; diff --git a/nav2_loopback_sim/launch/loopback_sim_launch.py b/nav2_loopback_sim/launch/loopback_sim_launch.py index 36aa0b9cd1..da222aa72c 100644 --- a/nav2_loopback_sim/launch/loopback_sim_launch.py +++ b/nav2_loopback_sim/launch/loopback_sim_launch.py @@ -17,12 +17,15 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import GroupAction, 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 +from launch_ros.actions import Node, PushROSNamespace +#from launch_ros.actions import PushRosNamespace +from nav2_common.launch import RewrittenYaml, ReplaceString +from launch_ros.descriptions import ParameterFile + def generate_launch_description(): # Get the launch directory @@ -31,11 +34,14 @@ def generate_launch_description(): # Create the launch configuration variables namespace = LaunchConfiguration('namespace') - #use_namespace = LaunchConfiguration('use_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') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -43,6 +49,9 @@ def generate_launch_description(): use_rviz = LaunchConfiguration('use_rviz') #headless = LaunchConfiguration('headless') #world = LaunchConfiguration('world') + robot_name = LaunchConfiguration('robot_name') + robot_sdf = LaunchConfiguration('robot_sdf') + # 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 @@ -52,16 +61,26 @@ def generate_launch_description(): # https://github.com/ros2/launch_ros/issues/56 remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + params_file = ReplaceString( + source_file=params_file, + replacements={'': ('/', namespace)}, + condition=IfCondition(use_namespace), + ) param_substitutions = { - 'use_sim_time': use_sim_time, + '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) + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) lifecycle_nodes = ['map_server'] @@ -95,6 +114,22 @@ def generate_launch_description(): declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='True', + description='Whether to use composed bringup', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', description='log level' + ) declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', @@ -142,29 +177,49 @@ def generate_launch_description(): 'use_namespace': 'False', 'rviz_config': rviz_config_file}.items()) - - nav_cmd = IncludeLaunchDescription( + bringup_cmd_group = GroupAction( + [ + PushROSNamespace(condition=IfCondition(use_namespace), namespace=namespace), + Node( + condition=IfCondition(use_composition), + name='nav2_container', + package='rclcpp_components', + executable='component_container_isolated', + parameters=[configured_params, {'autostart': autostart}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + output='screen', + ), + 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_composition': use_composition, + 'use_respawn': use_respawn, 'use_lifecycle_mgr': 'false', - 'map_subscribe_transient_local': 'true'}.items()) + '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}]) + parameters=[{'timer_frequency': 1.0}, + {'dt': 0.1}]) map_server_cmd = Node( package='nav2_map_server', executable='map_server', name='map_server', output='screen', - parameters=[configured_params], + parameters=[configured_params, + {'yaml_filename': map_yaml_file}], remappings=remappings) lifecycle_cmd = Node( @@ -186,18 +241,20 @@ def generate_launch_description(): 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_use_composition_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_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(bringup_cmd_group) ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(map_server_cmd) ld.add_action(lifecycle_cmd) + ld.add_action(loopback_sim_node) return ld \ No newline at end of file diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index 07099ecb28..7cd03ef068 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -16,6 +16,7 @@ rclcpp geometry_msgs std_msgs + sensor_msgs tf2 tf2_ros tf2_geometry_msgs diff --git a/nav2_loopback_sim/src/loopback_simulator.cpp b/nav2_loopback_sim/src/loopback_simulator.cpp index 64d33e0be8..ef9b051860 100644 --- a/nav2_loopback_sim/src/loopback_simulator.cpp +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include + #include "nav2_loopback_sim/loopback_simulator.hpp" namespace nav2_loopback_sim @@ -29,11 +30,18 @@ LoopbackSimulator::LoopbackSimulator(const rclcpp::NodeOptions & options) "initialpose", 10, std::bind(&LoopbackSimulator::initPoseCallback, this, std::placeholders::_1)); + // Initialize publishers + scan_publisher_ = this->create_publisher("scan", 10); + // Frequency parameter for timer function double timer_frequency{0.0}; declare_parameter("timer_frequency", 10.0); get_parameter("timer_frequency", timer_frequency); + // Delta time parameter for forward integration + declare_parameter("dt", 0.1); + get_parameter("dt", dt_); + // Create a timer timer_ = create_wall_timer( std::chrono::duration(1.0 / timer_frequency), @@ -51,74 +59,69 @@ LoopbackSimulator::~LoopbackSimulator() } +void LoopbackSimulator::publishTransform( + const geometry_msgs::msg::Pose & pose, + const std::string & parent_frame, + const std::string & child_frame) +{ + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = this->now() + rclcpp::Duration::from_seconds(dt_); + transformStamped.header.frame_id = parent_frame; + transformStamped.child_frame_id = child_frame; + transformStamped.transform.translation.x = pose.position.x; + transformStamped.transform.translation.y = pose.position.y; + transformStamped.transform.translation.z = pose.position.z; + transformStamped.transform.rotation = pose.orientation; + + tf_broadcaster_->sendTransform(transformStamped); +} + void LoopbackSimulator::twistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { - if (init_pose_set_ == false) - { - RCLCPP_WARN(rclcpp::get_logger("logger_name"), "Initial pose has not been set. Ignoring incoming velocity messages."); + if (!init_pose_set_) { + RCLCPP_WARN( + rclcpp::get_logger("logger_name"), + "Initial pose has not been set. Ignoring incoming velocity messages."); return; } - double dt = 0.1; // Adjust the time step - - // Transform initpose from map to odom frame - if (transform_initpose_once_ == true) { - try { - geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( - "odom", - init_pose_.header.frame_id, - tf2::TimePointZero); - - tf2::doTransform(init_pose_, odom_updated_pose_, transform); - } catch (const tf2::TransformException & ex) { - RCLCPP_ERROR( - rclcpp::get_logger("logger_name"), - "Could not transform init pose from Map to Odom."); - } - - transform_initpose_once_ = false; + // get base_link location in the odom frame + geometry_msgs::msg::TransformStamped transformStamped; + try { + transformStamped = tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; } - // Update the transformed pose in the local variable - odom_updated_pose_.header.frame_id = "odom"; - odom_updated_pose_.header.stamp = this->now(); - - tf2::Quaternion base_link_orientation( - odom_updated_pose_.pose.pose.orientation.x, - odom_updated_pose_.pose.pose.orientation.y, - odom_updated_pose_.pose.pose.orientation.z, - odom_updated_pose_.pose.pose.orientation.w - ); + // Convert the quaternion in the transform to tf2::Quaternion + tf2::Quaternion current_orientation; + tf2::fromMsg(transformStamped.transform.rotation, current_orientation); + // Calculate the change in orientation based on the twist message tf2::Quaternion angular_change; - angular_change.setRPY(msg->angular.x * dt, msg->angular.y * dt, msg->angular.z * dt); - base_link_orientation *= angular_change; + angular_change.setRPY(msg->angular.x * dt_, msg->angular.y * dt_, msg->angular.z * dt_); - odom_updated_pose_.pose.pose.orientation.x = base_link_orientation.x(); - odom_updated_pose_.pose.pose.orientation.y = base_link_orientation.y(); - odom_updated_pose_.pose.pose.orientation.z = base_link_orientation.z(); - odom_updated_pose_.pose.pose.orientation.w = base_link_orientation.w(); + // Integrate the change in orientation with the current orientation + tf2::Quaternion new_orientation = current_orientation * angular_change; - // Forward integrate robot position and orientation using velocity commands - tf2::Matrix3x3 rotation_matrix(base_link_orientation); + // Calculate the change in position based on the twist message + tf2::Matrix3x3 rotation_matrix(new_orientation); tf2::Vector3 linear_velocity(msg->linear.x, msg->linear.y, msg->linear.z); tf2::Vector3 rotated_linear_velocity = rotation_matrix * linear_velocity; - odom_updated_pose_.pose.pose.position.x += rotated_linear_velocity.x() * dt; - odom_updated_pose_.pose.pose.position.y += rotated_linear_velocity.y() * dt; - odom_updated_pose_.pose.pose.position.z += rotated_linear_velocity.z() * dt; - - // Broadcast "odom" to "base_link" transform using the local variable - geometry_msgs::msg::TransformStamped odom_to_base_transform; - odom_to_base_transform.header.stamp = odom_updated_pose_.header.stamp; - odom_to_base_transform.header.frame_id = "odom"; - odom_to_base_transform.child_frame_id = "base_link"; - odom_to_base_transform.transform.translation.x = odom_updated_pose_.pose.pose.position.x; - odom_to_base_transform.transform.translation.y = odom_updated_pose_.pose.pose.position.y; - odom_to_base_transform.transform.translation.z = odom_updated_pose_.pose.pose.position.z; - odom_to_base_transform.transform.rotation = odom_updated_pose_.pose.pose.orientation; - - tf_broadcaster_->sendTransform(odom_to_base_transform); + // Integrate the change in position with the current position + geometry_msgs::msg::Pose new_position; + new_position.position.x = + transformStamped.transform.translation.x + rotated_linear_velocity.x() * dt_; + new_position.position.y = + transformStamped.transform.translation.y + rotated_linear_velocity.y() * dt_; + new_position.position.z = + transformStamped.transform.translation.z + rotated_linear_velocity.z() * dt_; + new_position.orientation = tf2::toMsg(new_orientation); + + // Publish the updated transform between odom and base_link + publishTransform(new_position, "odom", "base_link"); } void LoopbackSimulator::initPoseCallback( @@ -129,41 +132,89 @@ void LoopbackSimulator::initPoseCallback( init_pose_ = *msg; init_pose_set_ = true; - if (init_odom_base_published_ == false) { - // Publish Identity transform between odom to base_footprint - geometry_msgs::msg::TransformStamped odom_to_base_transform; - odom_to_base_transform.header.stamp = this->now(); - odom_to_base_transform.header.frame_id = "odom"; - odom_to_base_transform.child_frame_id = "base_footprint"; - odom_to_base_transform.transform.translation.x = 0.0; - odom_to_base_transform.transform.translation.y = 0.0; - odom_to_base_transform.transform.translation.z = 0.0; - odom_to_base_transform.transform.rotation.x = 0.0; - odom_to_base_transform.transform.rotation.y = 0.0; - odom_to_base_transform.transform.rotation.z = 0.0; - odom_to_base_transform.transform.rotation.w = 1.0; - - tf_broadcaster_->sendTransform(odom_to_base_transform); - init_odom_base_published_ = true; + if (!zero_odom_base_published_) { + // Publish first init pose between map and odom + publishTransform(init_pose_.pose.pose, "map", "odom"); + geometry_msgs::msg::Pose zero_pose{}; + + // Publish identity transform between odom and base_footprint + publishTransform(zero_pose, "odom", "base_footprint"); + + relocalization_pose_ = init_pose_.pose.pose; + zero_odom_base_published_ = true; + return; } + + // Updating map->base_link transform based on set pose + geometry_msgs::msg::TransformStamped map_to_base_link_transform; + map_to_base_link_transform.header = init_pose_.header; + map_to_base_link_transform.header.frame_id = "map"; + map_to_base_link_transform.child_frame_id = "base_link"; + map_to_base_link_transform.transform.translation.x = init_pose_.pose.pose.position.x; + map_to_base_link_transform.transform.translation.y = init_pose_.pose.pose.position.y; + map_to_base_link_transform.transform.translation.z = init_pose_.pose.pose.position.z; + map_to_base_link_transform.transform.rotation = init_pose_.pose.pose.orientation; + + geometry_msgs::msg::TransformStamped odom_to_base_link_transform; + try { + odom_to_base_link_transform = tf_buffer_->lookupTransform( + "odom", + "base_link", + tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; + } + + // Calculating map->odom transform + geometry_msgs::msg::TransformStamped map_to_odom_transform; + tf2::doTransform(map_to_base_link_transform, map_to_odom_transform, odom_to_base_link_transform); + + // Updating relocalization pose of odom frame + relocalization_pose_.position.x = map_to_odom_transform.transform.translation.x; + relocalization_pose_.position.y = map_to_odom_transform.transform.translation.y; + relocalization_pose_.position.z = map_to_odom_transform.transform.translation.z; + relocalization_pose_.orientation = map_to_odom_transform.transform.rotation; + + // Publishing updated position of odom and base_link wrt map + publishTransform(relocalization_pose_, "map", "odom"); + publishTransform(init_pose_.pose.pose, "map", "base_link"); } void LoopbackSimulator::timerCallback() { - if (init_pose_set_ == false) {return;} + if (!init_pose_set_) { + return; + } + // RCLCPP_INFO(this->get_logger(), "Publishing map->odom tf"); - // Publish map to odom transform - geometry_msgs::msg::TransformStamped map_to_odom_transform; - map_to_odom_transform.header.stamp = this->now(); - map_to_odom_transform.header.frame_id = "map"; - map_to_odom_transform.child_frame_id = "odom"; - map_to_odom_transform.transform.translation.x = init_pose_.pose.pose.position.x; - map_to_odom_transform.transform.translation.y = init_pose_.pose.pose.position.y; - map_to_odom_transform.transform.translation.z = init_pose_.pose.pose.position.z; - map_to_odom_transform.transform.rotation = init_pose_.pose.pose.orientation; - - tf_broadcaster_->sendTransform(map_to_odom_transform); + // Continously publishing map->odom transform + publishTransform(relocalization_pose_, "map", "odom"); + + // publish bogus lidar data + auto scan_msg = std::make_shared(); + + scan_msg->header.frame_id = "base_link"; + scan_msg->header.stamp = this->now(); + + // Fill in the scan data + scan_msg->angle_min = -M_PI / 2.0; + scan_msg->angle_max = M_PI / 2.0; + scan_msg->angle_increment = M_PI / 180.0; + scan_msg->time_increment = 0.0; + scan_msg->scan_time = 0.1; + scan_msg->range_min = 0.0; + scan_msg->range_max = 10.0; + + // Fill in the scan data + int num_readings = 180; + scan_msg->ranges.resize(num_readings); + for (int i = 0; i < num_readings; ++i) { + scan_msg->ranges[i] = 5.0; // Set all ranges to 5 meters + } + + scan_publisher_->publish(*scan_msg); } } // namespace nav2_loopback_sim diff --git a/nav2_loopback_sim/src/main.cpp b/nav2_loopback_sim/src/main.cpp index 8db99d2635..c4ff29f9ec 100644 --- a/nav2_loopback_sim/src/main.cpp +++ b/nav2_loopback_sim/src/main.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include + #include "nav2_loopback_sim/loopback_simulator.hpp" int main(int argc, char ** argv) @@ -21,5 +22,6 @@ int main(int argc, char ** argv) auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); + return 0; } diff --git a/nav2_loopback_sim/test/CMakeLists.txt b/nav2_loopback_sim/test/CMakeLists.txt new file mode 100644 index 0000000000..0a8f7b5170 --- /dev/null +++ b/nav2_loopback_sim/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# Tests +ament_add_gtest(loopback_simulator_tests + test_loopback_simulator.cpp +) +ament_target_dependencies(loopback_simulator_tests + ${dependencies} +) +target_link_libraries(loopback_simulator_tests + ${library_name} +) \ No newline at end of file diff --git a/nav2_loopback_sim/test/test_loopback_simulator.cpp b/nav2_loopback_sim/test/test_loopback_simulator.cpp new file mode 100644 index 0000000000..e17bc93b1d --- /dev/null +++ b/nav2_loopback_sim/test/test_loopback_simulator.cpp @@ -0,0 +1,109 @@ +// Copyright (c) 2023 Smit Dumore +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_loopback_sim/loopback_simulator.hpp" + + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class LoopbackSimulatorTest : public nav2_loopback_sim::LoopbackSimulator { +public: + LoopbackSimulatorTest() + : nav2_loopback_sim::LoopbackSimulator(rclcpp::NodeOptions()) + { + // tf_buffer_ = std::make_shared(node_->get_clock()); + // tf_listener_ = std::make_shared(*tf_buffer_); + + // // Initialize publisher for cmd_vel + // cmd_vel_publisher_ = node_->create_publisher("cmd_vel", 10); + + // // Initilize a publisher for initial pose + // initial_pose_publisher_ = node_->create_publisher( + // "/initialpose", 10); + + // // Wait for the publisher to establish connections + // std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + rclcpp::Publisher::SharedPtr cmd_vel_publisher_; + rclcpp::Publisher::SharedPtr initial_pose_publisher_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; + +TEST(RobotSimulatorTest, TFUpdates) { + + auto node = std::make_shared(); + + node->tf_buffer_ = std::make_shared(node->get_clock()); + node->tf_listener_ = std::make_shared(*(node->tf_buffer_)); + node->cmd_vel_publisher_ = node->create_publisher("cmd_vel", 10); + node->initial_pose_publisher_ = node->create_publisher( + "/initialpose", 10); + + // Publish the initial pose + auto initial_pose_msg = std::make_shared(); + initial_pose_msg->pose.pose.position.x = 1.0; + initial_pose_msg->pose.pose.position.y = 1.0; + initial_pose_msg->pose.pose.position.z = 0.0; + initial_pose_msg->pose.pose.orientation.x = 0.0; + initial_pose_msg->pose.pose.orientation.y = 0.0; + initial_pose_msg->pose.pose.orientation.z = 0.0; + initial_pose_msg->pose.pose.orientation.w = 1.0; + node->initial_pose_publisher_->publish(*initial_pose_msg); + + + // Publish cmd_vel messages + geometry_msgs::msg::Twist cmd_vel_msg; + cmd_vel_msg.linear.x = 0.5; + cmd_vel_msg.angular.z = 0.2; + node->cmd_vel_publisher_->publish(cmd_vel_msg); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + try { + node->tf_buffer_->lookupTransform("base_link", "map", tf2::TimePointZero); + } catch (tf2::TransformException &ex) { + FAIL() << "TF lookup failed: " << ex.what(); + } +} + +// TEST_F(RobotSimulatorTest, Relocalization) { +// // Reset robot's pose to initial pose +// // Send command to reset pose or directly manipulate TF tree + +// // Wait for the update to be processed +// std::this_thread::sleep_for(std::chrono::seconds(1)); // Adjust sleep time as needed + +// // Check if the robot is correctly relocalized +// // Assert conditions on the pose +// // Example: ASSERT_NEAR(pose.x, initial_pose.x, tolerance); +// } + +// int main(int argc, char **argv) { +// ::testing::InitGoogleTest(&argc, argv); +// return RUN_ALL_TESTS(); +// } From fbf8b2b0bcf5fb37d6b4518dfe066c1fba2693bd Mon Sep 17 00:00:00 2001 From: smitdumore Date: Wed, 6 Mar 2024 00:09:31 -0500 Subject: [PATCH 11/12] added test cpp file and formatted launch file acc. to flake8 --- .../launch/loopback_sim_launch.py | 60 +++--- .../test/test_loopback_simulator.cpp | 171 ++++++++++-------- 2 files changed, 129 insertions(+), 102 deletions(-) diff --git a/nav2_loopback_sim/launch/loopback_sim_launch.py b/nav2_loopback_sim/launch/loopback_sim_launch.py index da222aa72c..745c047ab7 100644 --- a/nav2_loopback_sim/launch/loopback_sim_launch.py +++ b/nav2_loopback_sim/launch/loopback_sim_launch.py @@ -17,14 +17,15 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import GroupAction, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node, PushROSNamespace -#from launch_ros.actions import PushRosNamespace -from nav2_common.launch import RewrittenYaml, ReplaceString +# from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ParameterFile +from nav2_common.launch import ReplaceString, RewrittenYaml def generate_launch_description(): @@ -47,31 +48,26 @@ def generate_launch_description(): 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') - robot_name = LaunchConfiguration('robot_name') - robot_sdf = LaunchConfiguration('robot_sdf') + # 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 + # 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')] - + params_file = ReplaceString( source_file=params_file, replacements={'': ('/', namespace)}, condition=IfCondition(use_namespace), ) - param_substitutions = { - 'use_sim_time': use_sim_time, - 'yaml_filename': map_yaml_file} - configured_params = ParameterFile( RewrittenYaml( source_file=params_file, @@ -81,7 +77,7 @@ def generate_launch_description(): ), allow_substs=True, ) - + lifecycle_nodes = ['map_server'] # Declare the launch arguments @@ -114,7 +110,7 @@ def generate_launch_description(): declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') - + declare_use_composition_cmd = DeclareLaunchArgument( 'use_composition', default_value='True', @@ -191,20 +187,24 @@ def generate_launch_description(): output='screen', ), 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_composition': use_composition, - 'use_respawn': use_respawn, - 'use_lifecycle_mgr': 'false', - 'map_subscribe_transient_local': 'true'}.items(), + 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_composition': use_composition, + 'use_respawn': use_respawn, + 'use_lifecycle_mgr': 'false', + 'map_subscribe_transient_local': 'true' + }.items(), ), ] ) - + loopback_sim_node = Node( package='nav2_loopback_sim', executable='sim_node', @@ -219,7 +219,7 @@ def generate_launch_description(): name='map_server', output='screen', parameters=[configured_params, - {'yaml_filename': map_yaml_file}], + {'yaml_filename': map_yaml_file}], remappings=remappings) lifecycle_cmd = Node( @@ -257,4 +257,4 @@ def generate_launch_description(): ld.add_action(lifecycle_cmd) ld.add_action(loopback_sim_node) - return ld \ No newline at end of file + return ld diff --git a/nav2_loopback_sim/test/test_loopback_simulator.cpp b/nav2_loopback_sim/test/test_loopback_simulator.cpp index e17bc93b1d..94a15b037d 100644 --- a/nav2_loopback_sim/test/test_loopback_simulator.cpp +++ b/nav2_loopback_sim/test/test_loopback_simulator.cpp @@ -29,81 +29,108 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; -class LoopbackSimulatorTest : public nav2_loopback_sim::LoopbackSimulator { +class LoopbackSimulatorTest : public nav2_loopback_sim::LoopbackSimulator +{ public: - LoopbackSimulatorTest() - : nav2_loopback_sim::LoopbackSimulator(rclcpp::NodeOptions()) - { - // tf_buffer_ = std::make_shared(node_->get_clock()); - // tf_listener_ = std::make_shared(*tf_buffer_); - - // // Initialize publisher for cmd_vel - // cmd_vel_publisher_ = node_->create_publisher("cmd_vel", 10); - - // // Initilize a publisher for initial pose - // initial_pose_publisher_ = node_->create_publisher( - // "/initialpose", 10); - - // // Wait for the publisher to establish connections - // std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - rclcpp::Publisher::SharedPtr cmd_vel_publisher_; - rclcpp::Publisher::SharedPtr initial_pose_publisher_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; + LoopbackSimulatorTest() + : nav2_loopback_sim::LoopbackSimulator(rclcpp::NodeOptions()) + { + } + + rclcpp::Publisher::SharedPtr cmd_vel_publisher_; + + rclcpp::Publisher::SharedPtr + initial_pose_publisher_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr tf_broadcaster_; }; TEST(RobotSimulatorTest, TFUpdates) { - - auto node = std::make_shared(); - - node->tf_buffer_ = std::make_shared(node->get_clock()); - node->tf_listener_ = std::make_shared(*(node->tf_buffer_)); - node->cmd_vel_publisher_ = node->create_publisher("cmd_vel", 10); - node->initial_pose_publisher_ = node->create_publisher( - "/initialpose", 10); - - // Publish the initial pose - auto initial_pose_msg = std::make_shared(); - initial_pose_msg->pose.pose.position.x = 1.0; - initial_pose_msg->pose.pose.position.y = 1.0; - initial_pose_msg->pose.pose.position.z = 0.0; - initial_pose_msg->pose.pose.orientation.x = 0.0; - initial_pose_msg->pose.pose.orientation.y = 0.0; - initial_pose_msg->pose.pose.orientation.z = 0.0; - initial_pose_msg->pose.pose.orientation.w = 1.0; - node->initial_pose_publisher_->publish(*initial_pose_msg); - - - // Publish cmd_vel messages - geometry_msgs::msg::Twist cmd_vel_msg; - cmd_vel_msg.linear.x = 0.5; - cmd_vel_msg.angular.z = 0.2; - node->cmd_vel_publisher_->publish(cmd_vel_msg); - - std::this_thread::sleep_for(std::chrono::seconds(1)); - - try { - node->tf_buffer_->lookupTransform("base_link", "map", tf2::TimePointZero); - } catch (tf2::TransformException &ex) { - FAIL() << "TF lookup failed: " << ex.what(); - } + // auto node = std::make_shared(); + // rclcpp::executors::SingleThreadedExecutor executor; + // executor.add_node(node); + + + // node->tf_buffer_ = std::make_shared(node->get_clock()); + // node->tf_listener_ = std::make_shared(*(node->tf_buffer_)); + // node->tf_broadcaster_ = std::make_shared(*(node)); + // node->cmd_vel_publisher_ = node->create_publisher("cmd_vel", 10); + // node->initial_pose_publisher_ = node->create_publisher + // ("initialpose", 10); + + // //std::this_thread::sleep_for(std::chrono::seconds(5)); + + // geometry_msgs::msg::TransformStamped transform_stamped; + // transform_stamped.header.stamp = node->now(); + // transform_stamped.header.frame_id = "map"; + // transform_stamped.child_frame_id = "odom"; + // transform_stamped.transform.translation.x = 5.0; + // transform_stamped.transform.translation.y = 5.0; + // transform_stamped.transform.translation.z = 0.0; + // transform_stamped.transform.rotation.x = 0.0; + // transform_stamped.transform.rotation.y = 0.0; + // transform_stamped.transform.rotation.z = 0.0; + // transform_stamped.transform.rotation.w = 1.0; + // node->tf_broadcaster_->sendTransform(transform_stamped); + + // geometry_msgs::msg::TransformStamped transform_stamped; + // transform_stamped.header.stamp = node->now(); + // transform_stamped.header.frame_id = "odom"; + // transform_stamped.child_frame_id = "base_link"; + // transform_stamped.transform.translation.x = 0.0; + // transform_stamped.transform.translation.y = 0.0; + // transform_stamped.transform.translation.z = 0.0; + // transform_stamped.transform.rotation.x = 0.0; + // transform_stamped.transform.rotation.y = 0.0; + // transform_stamped.transform.rotation.z = 0.0; + // transform_stamped.transform.rotation.w = 1.0; + // node->tf_broadcaster_->sendTransform(transform_stamped); + + + // Publish the initial pose + // auto initial_pose_msg = std::make_shared(); + + // // Set the frame ID + // initial_pose_msg->header.frame_id = "map"; + + // // Set the timestamp + // initial_pose_msg->header.stamp = node->now(); + + // initial_pose_msg->pose.pose.position.x = 1.0; + // initial_pose_msg->pose.pose.position.y = 1.0; + // initial_pose_msg->pose.pose.position.z = 0.0; + // initial_pose_msg->pose.pose.orientation.x = 0.0; + // initial_pose_msg->pose.pose.orientation.y = 0.0; + // initial_pose_msg->pose.pose.orientation.z = 0.0; + // initial_pose_msg->pose.pose.orientation.w = 1.0; + // node->initial_pose_publisher_->publish(*initial_pose_msg); + + + // Publish cmd_vel messages + // geometry_msgs::msg::Twist cmd_vel_msg; + // cmd_vel_msg.linear.x = 0.5; + // cmd_vel_msg.angular.z = 0.2; + // std::cout << "publishing velo %\n"; + // node->cmd_vel_publisher_->publish(cmd_vel_msg); + + // try { + // geometry_msgs::msg::TransformStamped transformStamped = node->tf_buffer_->lookupTransform("map", "odom", tf2::TimePointZero); + + // EXPECT_NEAR(transformStamped.transform.translation.x, 1.0, 0.01); + // EXPECT_NEAR(transformStamped.transform.translation.y, 1.0, 0.01); + // } catch (tf2::TransformException & ex) { + // EXPECT_TRUE(false) << "TF lookup failed: " << ex.what(); + // } + + //executor.spin(); + //executor.spin_some(std::chrono::seconds(10)); + //rclcpp::shutdown(); } -// TEST_F(RobotSimulatorTest, Relocalization) { -// // Reset robot's pose to initial pose -// // Send command to reset pose or directly manipulate TF tree - -// // Wait for the update to be processed -// std::this_thread::sleep_for(std::chrono::seconds(1)); // Adjust sleep time as needed - -// // Check if the robot is correctly relocalized -// // Assert conditions on the pose -// // Example: ASSERT_NEAR(pose.x, initial_pose.x, tolerance); -// } - -// int main(int argc, char **argv) { -// ::testing::InitGoogleTest(&argc, argv); -// return RUN_ALL_TESTS(); -// } +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} \ No newline at end of file From 9f9f8002da44f86579b2109ea97b0f2c96079f29 Mon Sep 17 00:00:00 2001 From: smitdumore Date: Fri, 22 Mar 2024 18:35:36 -0400 Subject: [PATCH 12/12] fixed code formatting --- nav2_loopback_sim/CMakeLists.txt | 2 +- .../test/test_loopback_simulator.cpp | 26 ++++++++++++------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/nav2_loopback_sim/CMakeLists.txt b/nav2_loopback_sim/CMakeLists.txt index 850b22548b..6927fbbc18 100644 --- a/nav2_loopback_sim/CMakeLists.txt +++ b/nav2_loopback_sim/CMakeLists.txt @@ -42,7 +42,7 @@ add_executable(${executable_name} src/main.cpp ) -target_link_libraries(${executable_name} ${library_name}) +target_link_libraries(${executable_name} ${library_name}) ament_target_dependencies(${executable_name} ${dependencies} diff --git a/nav2_loopback_sim/test/test_loopback_simulator.cpp b/nav2_loopback_sim/test/test_loopback_simulator.cpp index 94a15b037d..5e2fb94ace 100644 --- a/nav2_loopback_sim/test/test_loopback_simulator.cpp +++ b/nav2_loopback_sim/test/test_loopback_simulator.cpp @@ -54,9 +54,12 @@ TEST(RobotSimulatorTest, TFUpdates) { // node->tf_buffer_ = std::make_shared(node->get_clock()); - // node->tf_listener_ = std::make_shared(*(node->tf_buffer_)); - // node->tf_broadcaster_ = std::make_shared(*(node)); - // node->cmd_vel_publisher_ = node->create_publisher("cmd_vel", 10); + // node->tf_listener_ = std::make_shared + // (*(node->tf_buffer_)); + // node->tf_broadcaster_ = std::make_shared + // (*(node)); + // node->cmd_vel_publisher_ = node->create_publisher + // ("cmd_vel", 10); // node->initial_pose_publisher_ = node->create_publisher // ("initialpose", 10); @@ -90,7 +93,8 @@ TEST(RobotSimulatorTest, TFUpdates) { // Publish the initial pose - // auto initial_pose_msg = std::make_shared(); + // auto initial_pose_msg = std::make_shared + // (); // // Set the frame ID // initial_pose_msg->header.frame_id = "map"; @@ -116,7 +120,8 @@ TEST(RobotSimulatorTest, TFUpdates) { // node->cmd_vel_publisher_->publish(cmd_vel_msg); // try { - // geometry_msgs::msg::TransformStamped transformStamped = node->tf_buffer_->lookupTransform("map", "odom", tf2::TimePointZero); + // geometry_msgs::msg::TransformStamped transformStamped = + // node->tf_buffer_->lookupTransform("map", "odom", tf2::TimePointZero); // EXPECT_NEAR(transformStamped.transform.translation.x, 1.0, 0.01); // EXPECT_NEAR(transformStamped.transform.translation.y, 1.0, 0.01); @@ -124,13 +129,14 @@ TEST(RobotSimulatorTest, TFUpdates) { // EXPECT_TRUE(false) << "TF lookup failed: " << ex.what(); // } - //executor.spin(); - //executor.spin_some(std::chrono::seconds(10)); - //rclcpp::shutdown(); + // executor.spin(); + // executor.spin_some(std::chrono::seconds(10)); + // rclcpp::shutdown(); } -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +}