diff --git a/nav2_loopback_sim/CMakeLists.txt b/nav2_loopback_sim/CMakeLists.txt new file mode 100644 index 0000000000..6927fbbc18 --- /dev/null +++ b/nav2_loopback_sim/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.5) +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(sensor_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 + rclcpp_components + std_msgs + sensor_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::LoopbackSimulator") + +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..9e2f053049 --- /dev/null +++ b/nav2_loopback_sim/include/nav2_loopback_sim/loopback_simulator.hpp @@ -0,0 +1,114 @@ +// 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. + +#ifndef NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_ +#define NAV2_LOOPBACK_SIM__LOOPBACK_SIMULATOR_HPP_ + +#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.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" +#include "tf2_ros/buffer.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/convert.h" +#include "tf2/exceptions.h" +#include "tf2/transform_datatypes.h" + +namespace nav2_loopback_sim +{ +/** + * @class nav2_loopback_sim::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_loopback_sim::LoopbackSimulator + */ + explicit LoopbackSimulator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** + * @brief A destructor for nav2_loopback_sim::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(); + + /** + * @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 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_; + 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..745c047ab7 --- /dev/null +++ b/nav2_loopback_sim/launch/loopback_sim_launch.py @@ -0,0 +1,260 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +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 +from launch_ros.actions import Node, PushROSNamespace +# from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import ReplaceString, 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') + 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') + 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')] + + params_file = ReplaceString( + source_file=params_file, + replacements={'': ('/', namespace)}, + condition=IfCondition(use_namespace), + ) + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=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_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', + 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()) + + 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(), + ), + + ] + ) + + loopback_sim_node = Node( + package='nav2_loopback_sim', + executable='sim_node', + name='loopback_simulator_node', + output='screen', + 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, + {'yaml_filename': map_yaml_file}], + 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_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) + + # 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 diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml new file mode 100644 index 0000000000..7cd03ef068 --- /dev/null +++ b/nav2_loopback_sim/package.xml @@ -0,0 +1,30 @@ + + + + nav2_loopback_sim + 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 + Smit Dumore + Apache-2.0 + + ament_cmake + + launch + + rclcpp + geometry_msgs + std_msgs + sensor_msgs + tf2 + tf2_ros + 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..ef9b051860 --- /dev/null +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -0,0 +1,227 @@ +// 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 + +#include "nav2_loopback_sim/loopback_simulator.hpp" + +namespace nav2_loopback_sim +{ + +LoopbackSimulator::LoopbackSimulator(const rclcpp::NodeOptions & options) +: Node("loopback_simulator_node", options) +{ + // 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)); + + // 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), + std::bind(&LoopbackSimulator::timerCallback, this)); + + // Transforms + tf_broadcaster_ = std::make_unique(*this); + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +LoopbackSimulator::~LoopbackSimulator() +{ + RCLCPP_INFO(get_logger(), "Destroying %s", get_name()); +} + + +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_) { + RCLCPP_WARN( + rclcpp::get_logger("logger_name"), + "Initial pose has not been set. Ignoring incoming velocity messages."); + return; + } + + // 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; + } + + // 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_); + + // Integrate the change in orientation with the current orientation + tf2::Quaternion new_orientation = current_orientation * angular_change; + + // 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; + + // 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( + const + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + RCLCPP_INFO(this->get_logger(), "Received initial pose!"); + init_pose_ = *msg; + init_pose_set_ = 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_) { + return; + } + + // RCLCPP_INFO(this->get_logger(), "Publishing map->odom tf"); + + // 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 + +#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) diff --git a/nav2_loopback_sim/src/main.cpp b/nav2_loopback_sim/src/main.cpp new file mode 100644 index 0000000000..c4ff29f9ec --- /dev/null +++ b/nav2_loopback_sim/src/main.cpp @@ -0,0 +1,27 @@ +// 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 + +#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/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..5e2fb94ace --- /dev/null +++ b/nav2_loopback_sim/test/test_loopback_simulator.cpp @@ -0,0 +1,142 @@ +// 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()) + { + } + + 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(); + // 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(); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} 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