diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 34aa6c532f0..62991ccb2ee 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.0.6 + 1.0.7

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 1f0ed2050d8..4b77c3f9eab 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -234,12 +234,12 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/) initParameters(); initTransforms(); + initParticleFilter(); + initLaserScan(); initMessageFilters(); initPubSub(); initServices(); initOdometry(); - initParticleFilter(); - initLaserScan(); return nav2_util::CallbackReturn::SUCCESS; } @@ -335,8 +335,10 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) laser_scan_sub_.reset(); // Map - map_free(map_); - map_ = nullptr; + if (map_ != NULL) { + map_free(map_); + map_ = nullptr; + } first_map_received_ = false; free_space_indices.resize(0); @@ -1105,6 +1107,25 @@ AmclNode::initParameters() last_time_printed_msg_ = now(); // Semantic checks + if (laser_likelihood_max_dist_ < 0) { + RCLCPP_WARN( + get_logger(), "You've set laser_likelihood_max_dist to be negtive," + " this isn't allowed so it will be set to default value 2.0."); + laser_likelihood_max_dist_ = 2.0; + } + if (max_particles_ < 0) { + RCLCPP_WARN( + get_logger(), "You've set max_particles to be negtive," + " this isn't allowed so it will be set to default value 2000."); + max_particles_ = 2000; + } + + if (min_particles_ < 0) { + RCLCPP_WARN( + get_logger(), "You've set min_particles to be negtive," + " this isn't allowed so it will be set to default value 500."); + min_particles_ = 500; + } if (min_particles_ > max_particles_) { RCLCPP_WARN( @@ -1113,6 +1134,13 @@ AmclNode::initParameters() max_particles_ = min_particles_; } + if (resample_interval_ <= 0) { + RCLCPP_WARN( + get_logger(), "You've set resample_interval to be zero or negtive," + " this isn't allowed so it will be set to default value to 1."); + resample_interval_ = 1; + } + if (always_reset_initial_pose_) { initial_pose_is_known_ = false; } @@ -1239,7 +1267,7 @@ AmclNode::initMessageFilters() rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data); laser_scan_filter_ = std::make_unique>( - *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_); + *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_); laser_scan_connection_ = laser_scan_filter_->registerCallback( std::bind( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 63be7034428..64f4ffa81b2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -82,11 +82,16 @@ bool BtActionServer::on_configure() throw std::runtime_error{"Failed to lock node"}; } - // use suffix '_rclcpp_node' to keep parameter file consistency #1773 + // Name client node after action name + std::string client_node_name = action_name_; + std::replace(client_node_name.begin(), client_node_name.end(), '/', '_'); + // Use suffix '_rclcpp_node' to keep parameter file consistency #1773 auto options = rclcpp::NodeOptions().arguments( {"--ros-args", - "-r", std::string("__node:=") + std::string(node->get_name()) + action_name_ + "_rclcpp_node", + "-r", + std::string("__node:=") + std::string(node->get_name()) + client_node_name + "_rclcpp_node", "--"}); + // Support for handling the topic-based goal pose from rviz client_node_ = std::make_shared("_", options); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 5317b9136ea..e557c559ce4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -142,6 +142,16 @@ class BtServiceNode : public BT::ActionNodeBase { } + /** + * @brief Function to perform some user-defined operation upon successful + * completion of the service. Could put a value on the blackboard. + * @return BT::NodeStatus Returns SUCCESS by default, user may override to return another value + */ + virtual BT::NodeStatus on_completion() + { + return BT::NodeStatus::SUCCESS; + } + /** * @brief Check the future and decide the status of BT * @return BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise @@ -158,7 +168,8 @@ class BtServiceNode : public BT::ActionNodeBase rc = callback_group_executor_.spin_until_future_complete(future_result_, server_timeout_); if (rc == rclcpp::FutureReturnCode::SUCCESS) { request_sent_ = false; - return BT::NodeStatus::SUCCESS; + BT::NodeStatus status = on_completion(); + return status; } if (rc == rclcpp::FutureReturnCode::TIMEOUT) { diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 703c7bdf07c..167823a99b4 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.0.6 + 1.0.7 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py index dd13fae8bbd..a8b355595aa 100644 --- a/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py +++ b/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py @@ -102,7 +102,8 @@ def generate_launch_description(): # Start Gazebo with plugin providing the robot spawing service start_gazebo_cmd = ExecuteProcess( - cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world], + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], output='screen') # Define commands for spawing the robots into Gazebo @@ -123,7 +124,7 @@ def generate_launch_description(): # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: - params_file = LaunchConfiguration(robot['name'] + '_params_file') + params_file = LaunchConfiguration(f"{robot['name']}_params_file") group = GroupAction([ IncludeLaunchDescription( diff --git a/nav2_bringup/bringup/launch/tb3_simulation_launch.py b/nav2_bringup/bringup/launch/tb3_simulation_launch.py index 5fad6db3f8a..675b6056980 100644 --- a/nav2_bringup/bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/bringup/launch/tb3_simulation_launch.py @@ -131,7 +131,7 @@ def generate_launch_description(): # Specify the actions start_gazebo_server_cmd = ExecuteProcess( condition=IfCondition(use_simulator), - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world], + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world], cwd=[launch_dir], output='screen') start_gazebo_client_cmd = ExecuteProcess( diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index 851693f41de..806097b5033 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.0.6 + 1.0.7 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 2c0d414abd9..f90053c032c 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -59,6 +59,10 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + # if enable_groot_monitoring is set to True, ports need to be different for each robot !! + enable_groot_monitoring: False + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_compute_path_through_poses_action_bt_node diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 9f671b246a3..dfb6ab4520c 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -59,6 +59,10 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + # if enable_groot_monitoring is set to True, ports need to be different for each robot !! + enable_groot_monitoring: False + groot_zmq_publisher_port: 1789 + groot_zmq_server_port: 1887 plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_compute_path_through_poses_action_bt_node diff --git a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py index 9f4daaf196c..c7ef149c42f 100644 --- a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py +++ b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py @@ -37,7 +37,8 @@ def main(): parser.add_argument('-z', type=float, default=0, help='the z component of the initial position [meters]') parser.add_argument('-k', '--timeout', type=float, default=10.0, - help="Seconds to wait. Block until the future is complete if negative. Don't wait if 0.") + help="Seconds to wait. Block until the future is complete if negative. \ + Don't wait if 0.") group = parser.add_mutually_exclusive_group(required=True) group.add_argument('-t', '--turtlebot_type', type=str, @@ -67,7 +68,7 @@ def main(): if args.turtlebot_type is not None: sdf_file_path = os.path.join( get_package_share_directory('turtlebot3_gazebo'), 'models', - 'turtlebot3_{}'.format(args.turtlebot_type), 'model.sdf') + f'turtlebot3_{args.turtlebot_type}', 'model.sdf') else: sdf_file_path = args.sdf @@ -78,8 +79,7 @@ def main(): tree = ET.parse(sdf_file_path) root = tree.getroot() for plugin in root.iter('plugin'): - # TODO(orduno) Handle case if an sdf file from non-turtlebot is provided - if 'turtlebot3_diff_drive' in plugin.attrib.values(): + if 'ros_diff_drive' in plugin.get('filename'): # The only plugin we care for now is 'diff_drive' which is # broadcasting a transform between`odom` and `base_footprint` break @@ -87,7 +87,7 @@ def main(): if args.robot_namespace: ros_params = plugin.find('ros') ros_tf_remap = ET.SubElement(ros_params, 'remapping') - ros_tf_remap.text = '/tf:=/' + args.robot_namespace + '/tf' + ros_tf_remap.text = f'/tf:=/{args.robot_namespace}/tf' # Set data for request request = SpawnEntity.Request() @@ -102,10 +102,10 @@ def main(): future = client.call_async(request) rclpy.spin_until_future_complete(node, future, timeout_sec=args.timeout) if future.result() is not None: - print('response: %r' % future.result()) + print(f'response: {future.result()!r}') else: raise RuntimeError( - 'exception while calling service: %r' % future.exception()) + f'exception while calling service: {future.exception()!r}') node.get_logger().info('Done! Shutting down node.') node.destroy_node() diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index ba2a942aca9..da6b470d4d9 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 1.0.6 + 1.0.7 Package for spawning a robot model into Gazebo for Nav2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index a41736d41d2..c889384fa83 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.0.6 + 1.0.7 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index f28a0b33623..3f7ac7cc482 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.0.6 + 1.0.7 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 5ca5a22605c..8a7b1da217e 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -110,6 +110,7 @@ ament_export_libraries(simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name}) +ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_core plugins.xml) ament_package() diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 4afedc019c0..254d47471e2 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.0.6 + 1.0.7 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 1c13117d5be..52f4c058278 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -71,6 +71,7 @@ ControllerServer::~ControllerServer() progress_checker_.reset(); goal_checkers_.clear(); controllers_.clear(); + costmap_thread_.reset(); } nav2_util::CallbackReturn diff --git a/nav2_core/package.xml b/nav2_core/package.xml index f1ed541a88c..54b55ee3b8d 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.0.6 + 1.0.7 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp index a0602b6865a..ddf64ffbdc1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp @@ -146,10 +146,8 @@ class LayeredCostmap /** * @brief Add a new plugin to the plugins vector to process */ - void addPlugin(std::shared_ptr plugin) - { - plugins_.push_back(plugin); - } + void addPlugin(std::shared_ptr plugin); + /** * @brief Add a new costmap filter plugin to the filters vector to process diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 8006e342d7b..d911ccad17d 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.0.6 + 1.0.7 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 11a79558ab5..0e33aafb3bc 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -218,7 +218,7 @@ void ObstacleLayer::onInitialize() std::shared_ptr> filter( new tf2_ros::MessageFilter( - *sub, *tf_, global_frame_, 50, rclcpp_node_)); + *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance))); if (inf_is_valid) { filter->registerCallback( @@ -252,7 +252,7 @@ void ObstacleLayer::onInitialize() std::shared_ptr> filter( new tf2_ros::MessageFilter( - *sub, *tf_, global_frame_, 50, rclcpp_node_)); + *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance))); filter->registerCallback( std::bind( @@ -291,6 +291,13 @@ ObstacleLayer::laserScanCallback( global_frame_.c_str(), ex.what()); projector_.projectLaser(*message, cloud); + } catch (std::runtime_error & ex) { + RCLCPP_WARN( + logger_, + "transformLaserScanToPointCloud error, it seems the message from laser is malformed." + " Ignore this message. what(): %s", + ex.what()); + return; } // buffer the point cloud @@ -327,6 +334,13 @@ ObstacleLayer::laserScanValidInfCallback( "High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), ex.what()); projector_.projectLaser(message, cloud); + } catch (std::runtime_error & ex) { + RCLCPP_WARN( + logger_, + "transformLaserScanToPointCloud error, it seems the message from laser is malformed." + " Ignore this message. what(): %s", + ex.what()); + return; } // buffer the point cloud diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index b8e5261bc14..51e70ab9d20 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -110,6 +110,7 @@ VoxelLayer::~VoxelLayer() void VoxelLayer::matchSize() { + std::lock_guard guard(*getMutex()); ObstacleLayer::matchSize(); voxel_grid_.resize(size_x_, size_y_, size_z_); assert(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 922478bb0dd..18039f4298f 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -129,8 +129,8 @@ void ClearCostmapService::clearLayerRegion( double end_point_y = start_point_y + reset_distance; int start_x, start_y, end_x, end_y; - costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y); - costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y); + costmap->worldToMapEnforceBounds(start_point_x, start_point_y, start_x, start_y); + costmap->worldToMapEnforceBounds(end_point_x, end_point_y, end_x, end_y); costmap->clearArea(start_x, start_y, end_x, end_y, invert); diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index 37b99bb5980..2c12cd104ba 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -55,30 +55,33 @@ std::shared_ptr CostmapSubscriber::getCostmap() void CostmapSubscriber::toCostmap2D() { + + auto current_costmap_msg = std::atomic_load(&costmap_msg_); + if (costmap_ == nullptr) { costmap_ = std::make_shared( - costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y, - costmap_msg_->metadata.resolution, costmap_msg_->metadata.origin.position.x, - costmap_msg_->metadata.origin.position.y); - } else if (costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x || // NOLINT - costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y || - costmap_->getResolution() != costmap_msg_->metadata.resolution || - costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x || - costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y) + current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, + current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x, + current_costmap_msg->metadata.origin.position.y); + } else if (costmap_->getSizeInCellsX() != current_costmap_msg->metadata.size_x || // NOLINT + costmap_->getSizeInCellsY() != current_costmap_msg->metadata.size_y || + costmap_->getResolution() != current_costmap_msg->metadata.resolution || + costmap_->getOriginX() != current_costmap_msg->metadata.origin.position.x || + costmap_->getOriginY() != current_costmap_msg->metadata.origin.position.y) { // Update the size of the costmap costmap_->resizeMap( - costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y, - costmap_msg_->metadata.resolution, - costmap_msg_->metadata.origin.position.x, - costmap_msg_->metadata.origin.position.y); + current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, + current_costmap_msg->metadata.resolution, + current_costmap_msg->metadata.origin.position.x, + current_costmap_msg->metadata.origin.position.y); } unsigned char * master_array = costmap_->getCharMap(); unsigned int index = 0; - for (unsigned int i = 0; i < costmap_msg_->metadata.size_x; ++i) { - for (unsigned int j = 0; j < costmap_msg_->metadata.size_y; ++j) { - master_array[index] = costmap_msg_->data[index]; + for (unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i) { + for (unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j) { + master_array[index] = current_costmap_msg->data[index]; ++index; } } @@ -86,7 +89,7 @@ void CostmapSubscriber::toCostmap2D() void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) { - costmap_msg_ = msg; + std::atomic_store(&costmap_msg_, msg); if (!costmap_received_) { costmap_received_ = true; } diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp index 29a275d0348..f87d7d7f1b1 100644 --- a/nav2_costmap_2d/src/footprint_subscriber.cpp +++ b/nav2_costmap_2d/src/footprint_subscriber.cpp @@ -59,9 +59,10 @@ FootprintSubscriber::getFootprint( return false; } + auto current_footprint = std::atomic_load(&footprint_); footprint = toPointVector( - std::make_shared(footprint_->polygon)); - auto & footprint_stamp = footprint_->header.stamp; + std::make_shared(current_footprint->polygon)); + auto & footprint_stamp = current_footprint->header.stamp; if (stamp - footprint_stamp > valid_footprint_timeout) { return false; @@ -89,7 +90,7 @@ FootprintSubscriber::getFootprint( void FootprintSubscriber::footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg) { - footprint_ = msg; + std::atomic_store(&footprint_, msg); if (!footprint_received_) { footprint_received_ = true; } diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index e31ebe1414c..bd3d37cd31e 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -89,6 +89,12 @@ LayeredCostmap::~LayeredCostmap() } } +void LayeredCostmap::addPlugin(std::shared_ptr plugin) +{ + std::unique_lock lock(*(combined_costmap_.getMutex())); + plugins_.push_back(plugin); +} + void LayeredCostmap::resizeMap( unsigned int size_x, unsigned int size_y, double resolution, double origin_x, diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index ab27087161f..7aa0b1a7227 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.0.6 + 1.0.7 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index c95de462381..81c39aee268 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -73,8 +73,19 @@ class DWBLocalPlanner : public nav2_core::Controller virtual ~DWBLocalPlanner() {} + /** + * @brief Activate lifecycle node + */ void activate() override; + + /** + * @brief Deactivate lifecycle node + */ void deactivate() override; + + /** + * @brief Cleanup lifecycle node + */ void cleanup() override; /** diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index b505df5cfd3..227977ba6b2 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.0.6 + 1.0.7 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 4f732af7e63..806dbebb588 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.0.6 + 1.0.7 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index c7df1e4a7e1..2fc35929859 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.0.6 + 1.0.7 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 3a4bca29a1e..c504b6185da 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.0.6 + 1.0.7 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 08d44cacedc..b78a50c78de 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.0.6 + 1.0.7 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 465bd5d145f..fd6eec7e1d0 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.0.6 + 1.0.7 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 153ac633e7c..8e0f2038218 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.0.6 + 1.0.7 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index c40ddfa5d02..daf6abd40d1 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -52,9 +53,9 @@ class LifecycleManager : public rclcpp::Node ~LifecycleManager(); protected: - // The ROS node to create bond - rclcpp::Node::SharedPtr bond_client_node_; - std::unique_ptr bond_node_thread_; + // Callback group used by services and timers + rclcpp::CallbackGroup::SharedPtr callback_group_; + std::unique_ptr service_thread_; // The services provided by this node rclcpp::Service::SharedPtr manager_srv_; diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 618799aa088..1a4b104a73d 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.0.6 + 1.0.7 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 9ca4dc1ebba..2d26f7c144f 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -49,18 +49,18 @@ LifecycleManager::LifecycleManager() bond_timeout_ = std::chrono::duration_cast( std::chrono::duration(bond_timeout_s)); + callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); manager_srv_ = create_service( get_name() + std::string("/manage_nodes"), - std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3)); + std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), + rmw_qos_profile_services_default, + callback_group_); is_active_srv_ = create_service( get_name() + std::string("/is_active"), - std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3)); - - auto bond_options = rclcpp::NodeOptions().arguments( - {"--ros-args", "-r", std::string("__node:=") + get_name() + "_bond_client", "--"}); - bond_client_node_ = std::make_shared("_", bond_options); - bond_node_thread_ = std::make_unique(bond_client_node_); + std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3), + rmw_qos_profile_services_default, + callback_group_); transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE; transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED; @@ -84,12 +84,17 @@ LifecycleManager::LifecycleManager() if (autostart_) { startup(); } - }); + }, + callback_group_); + auto executor = std::make_shared(); + executor->add_callback_group(callback_group_, get_node_base_interface()); + service_thread_ = std::make_unique(executor); } LifecycleManager::~LifecycleManager() { RCLCPP_INFO(get_logger(), "Destroying %s", get_name()); + service_thread_.reset(); } void @@ -154,7 +159,7 @@ LifecycleManager::createBondConnection(const std::string & node_name) if (bond_map_.find(node_name) == bond_map_.end() && bond_timeout_.count() > 0.0) { bond_map_[node_name] = - std::make_shared("bond", node_name, bond_client_node_); + std::make_shared("bond", node_name, shared_from_this()); bond_map_[node_name]->setHeartbeatTimeout(timeout_s); bond_map_[node_name]->setHeartbeatPeriod(0.10); bond_map_[node_name]->start(); @@ -317,7 +322,8 @@ LifecycleManager::createBondTimer() bond_timer_ = this->create_wall_timer( 200ms, - std::bind(&LifecycleManager::checkBondConnections, this)); + std::bind(&LifecycleManager::checkBondConnections, this), + callback_group_); } void diff --git a/nav2_lifecycle_manager/src/main.cpp b/nav2_lifecycle_manager/src/main.cpp index a07cfa42353..94268ae2f3c 100644 --- a/nav2_lifecycle_manager/src/main.cpp +++ b/nav2_lifecycle_manager/src/main.cpp @@ -21,7 +21,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - rclcpp::spin(node->get_node_base_interface()); + rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 644060d5b0f..8ffe285cb7b 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -154,4 +154,5 @@ ament_export_libraries( ${library_name} ${map_io_library_name} ) +ament_export_dependencies(${map_io_dependencies} ${map_server_dependencies}) ament_package() diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index d5b03941fba..ba01c8dbbf8 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.0.6 + 1.0.7 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 3761c35db97..97e7dddc1f4 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -42,7 +42,7 @@ using namespace std::placeholders; namespace nav2_map_server { MapSaver::MapSaver() -: nav2_util::LifecycleNode("map_saver", "", true) +: nav2_util::LifecycleNode("map_saver", "") { RCLCPP_INFO(get_logger(), "Creating"); @@ -149,12 +149,6 @@ bool MapSaver::saveMapTopicToFile( map_topic_loc.c_str(), save_parameters_loc.map_file_name.c_str()); try { - // Pointer to map message received in the subscription callback - nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = nullptr; - - // Mutex for handling map_msg shared resource - std::recursive_mutex access; - // Correct map_topic_loc if necessary if (map_topic_loc == "") { map_topic_loc = "map"; @@ -179,47 +173,51 @@ bool MapSaver::saveMapTopicToFile( save_parameters_loc.occupied_thresh = occupied_thresh_default_; } + std::promise prom; + std::future future_result = prom.get_future(); // A callback function that receives map message from subscribed topic - auto mapCallback = [&map_msg, &access]( + auto mapCallback = [&prom]( const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void { - std::lock_guard guard(access); - map_msg = msg; + prom.set_value(msg); }; - // Add new subscription for incoming map topic. - // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode - // as a map listener. rclcpp::QoS map_qos(10); // initialize to default if (map_subscribe_transient_local_) { map_qos.transient_local(); map_qos.reliable(); map_qos.keep_last(1); } - auto map_sub = rclcpp_node_->create_subscription( - map_topic_loc, map_qos, mapCallback); - - rclcpp::Time start_time = now(); - while (rclcpp::ok()) { - if ((now() - start_time) > *save_map_timeout_) { - RCLCPP_ERROR(get_logger(), "Failed to save the map: timeout"); - return false; - } - - if (map_msg) { - std::lock_guard guard(access); - // map_sub is no more needed - map_sub.reset(); - // Map message received. Saving it to file - if (saveMapToFile(*map_msg, save_parameters_loc)) { - RCLCPP_INFO(get_logger(), "Map saved successfully"); - return true; - } else { - RCLCPP_ERROR(get_logger(), "Failed to save the map"); - return false; - } - } - - rclcpp::sleep_for(std::chrono::milliseconds(100)); + + // Create new CallbackGroup for map_sub + auto callback_group = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + auto option = rclcpp::SubscriptionOptions(); + option.callback_group = callback_group; + auto map_sub = create_subscription( + map_topic_loc, map_qos, mapCallback, option); + + // Create SingleThreadedExecutor to spin map_sub in callback_group + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_callback_group(callback_group, get_node_base_interface()); + // Spin until map message received + auto timeout = save_map_timeout_->to_chrono(); + auto status = executor.spin_until_future_complete(future_result, timeout); + if (status != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Failed to spin map subscription"); + return false; + } + // map_sub is no more needed + map_sub.reset(); + // Map message received. Saving it to file + nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = future_result.get(); + if (saveMapToFile(*map_msg, save_parameters_loc)) { + RCLCPP_INFO(get_logger(), "Map saved successfully"); + return true; + } else { + RCLCPP_ERROR(get_logger(), "Failed to save the map"); + return false; } } catch (std::exception & e) { RCLCPP_ERROR(get_logger(), "Failed to save the map: %s", e.what()); diff --git a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py index 9c061439c77..7287f735941 100644 --- a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py +++ b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): - map_publisher = os.path.dirname(os.getenv('TEST_EXECUTABLE')) + '/test_map_saver_publisher' + map_publisher = f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher" ld = LaunchDescription() diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 67f228d5bb5..f7e7c2958d2 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.0.6 + 1.0.7 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 98180cdd17a..92d423b8a95 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -30,6 +30,7 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" namespace nav2_navfn_planner { @@ -205,7 +206,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner std::string global_frame_, name_; // Whether or not the planner should be allowed to plan through unknown space - bool allow_unknown_; + bool allow_unknown_, use_final_approach_orientation_; // If the goal is obstructed, the tolerance specifies how many meters the planner // can relax the constraint in x and y before failing diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 299602f4bd9..337772732fd 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.0.6 + 1.0.7 TODO Steve Macenski Carlos Orduno diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 4cdaab3b73b..d589a8b9c9b 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -85,6 +85,9 @@ NavfnPlanner::configure( node->get_parameter(name + ".use_astar", use_astar_); declare_parameter_if_not_declared(node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", allow_unknown_); + declare_parameter_if_not_declared( + node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_); // Create a planner based on the new costmap size planner_ = std::make_unique( @@ -144,12 +147,40 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( nav_msgs::msg::Path path; + // Corner case of the start(x,y) = goal(x,y) + if (start.pose.position.x == goal.pose.position.x && + start.pose.position.y == goal.pose.position.y) + { + unsigned int mx, my; + costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); + return path; + } + path.header.stamp = clock_->now(); + path.header.frame_id = global_frame_; + geometry_msgs::msg::PoseStamped pose; + pose.header = path.header; + pose.pose.position.z = 0.0; + + pose.pose = start.pose; + // if we have a different start and goal orientation, set the unique path pose to the goal + // orientation, unless use_final_approach_orientation=true where we need it to be the start + // orientation to avoid movement from the local planner + if (start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_) { + pose.pose.orientation = goal.pose.orientation; + } + path.poses.push_back(pose); + return path; + } + if (!makePlan(start.pose, goal.pose, tolerance_, path)) { RCLCPP_WARN( logger_, "%s: failed to create plan with " "tolerance %.2f.", name_.c_str(), tolerance_); } + #ifdef BENCHMARK_TESTING steady_clock::time_point b = steady_clock::now(); duration time_span = duration_cast>(b - a); @@ -284,6 +315,32 @@ NavfnPlanner::makePlan( // extract the plan if (getPlanFromPotential(best_pose, plan)) { smoothApproachToGoal(best_pose, plan); + + // If use_final_approach_orientation=true, interpolate the last pose orientation from the + // previous pose to set the orientation to the 'final approach' orientation of the robot so + // it does not rotate. + // And deal with corner case of plan of length 1 + if (use_final_approach_orientation_) { + size_t plan_size = plan.poses.size(); + if (plan_size == 1) { + plan.poses.back().pose.orientation = start.orientation; + } else if (plan_size > 1) { + double dx, dy, theta; + auto last_pose = plan.poses.back().pose.position; + auto approach_pose = plan.poses[plan_size - 2].pose.position; + // Deal with the case of NavFn producing a path with two equal last poses + if (std::abs(last_pose.x - approach_pose.x) < 0.0001 && + std::abs(last_pose.y - approach_pose.y) < 0.0001 && plan_size > 2) + { + approach_pose = plan.poses[plan_size - 3].pose.position; + } + dx = last_pose.x - approach_pose.x; + dy = last_pose.y - approach_pose.y; + theta = atan2(dy, dx); + plan.poses.back().pose.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(theta); + } + } } else { RCLCPP_ERROR( logger_, @@ -489,6 +546,8 @@ NavfnPlanner::on_parameter_event_callback( use_astar_ = value.bool_value; } else if (name == name_ + ".allow_unknown") { allow_unknown_ = value.bool_value; + } else if (name == name_ + ".use_final_approach_orientation") { + use_final_approach_orientation_ = value.bool_value; } } } diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 32a0675f7d8..01e8a54ad2d 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.0.6 + 1.0.7 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_recoveries/CMakeLists.txt index 01849b8c119..cfdcc8ff335 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/nav2_recoveries/CMakeLists.txt @@ -138,5 +138,5 @@ ament_export_libraries(${library_name} nav2_spin_recovery nav2_wait_recovery ) - +ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 9a2d15acd34..7c2d7432f60 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 1.0.6 + 1.0.7 TODO Carlos Orduno Steve Macenski diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index b2b7c107f1c..d09ff2b4bfd 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -232,6 +232,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::shared_ptr costmap_ros_; nav2_costmap_2d::Costmap2D * costmap_; rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; + rclcpp::Clock::SharedPtr clock_; double desired_linear_vel_, base_desired_linear_vel_; double lookahead_dist_; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 9a67103fc74..b8e627ac11d 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.0.6 + 1.0.7 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 10298bc3f45..38adc7030cb 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -51,6 +51,7 @@ void RegulatedPurePursuitController::configure( tf_ = tf; plugin_name_ = name; logger_ = node->get_logger(); + clock_ = node->get_clock(); double transform_tolerance = 0.1; double control_frequency = 20.0; @@ -436,7 +437,15 @@ bool RegulatedPurePursuitController::isCollisionImminent( bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) { unsigned int mx, my; - costmap_->worldToMap(x, y, mx, my); + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 30000, + "The dimensions of the costmap is too small to successfully check for " + "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " + "increase your costmap size."); + return false; + } unsigned char cost = costmap_->getCost(mx, my); @@ -450,7 +459,16 @@ bool RegulatedPurePursuitController::inCollision(const double & x, const double double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) { unsigned int mx, my; - costmap_->worldToMap(x, y, mx, my); + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_FATAL( + logger_, + "The dimensions of the costmap is too small to fully include your robot's footprint, " + "thusly the robot cannot proceed further"); + throw nav2_core::PlannerException( + "RegulatedPurePursuitController: Dimensions of the costmap are too small " + "to encapsulate the robot footprint at current speeds!"); + } unsigned char cost = costmap_->getCost(mx, my); return static_cast(cost); @@ -606,7 +624,7 @@ double RegulatedPurePursuitController::findDirectionChange( const geometry_msgs::msg::PoseStamped & pose) { // Iterating through the global path to determine the position of the cusp - for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size(); ++pose_id) { + for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size() - 1; ++pose_id) { // We have two vectors for the dot product OA and AB. Determining the vectors. double oa_x = global_plan_.poses[pose_id].pose.position.x - global_plan_.poses[pose_id - 1].pose.position.x; diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index cec3a1d3888..0371f496a90 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" @@ -91,6 +92,12 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel, sign); } + + double findDirectionChangeWrapper( + const geometry_msgs::msg::PoseStamped & pose) + { + return findDirectionChange(pose); + } }; TEST(RegulatedPurePursuitTest, basicAPI) @@ -144,6 +151,32 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) EXPECT_EQ(rtn->point.z, 0.01); } +TEST(RegulatedPurePursuitTest, findDirectionChange) +{ + auto ctrl = std::make_shared(); + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 1.0; + pose.pose.position.y = 0.0; + + nav_msgs::msg::Path path; + path.poses.resize(3); + path.poses[0].pose.position.x = 1.0; + path.poses[0].pose.position.y = 1.0; + path.poses[1].pose.position.x = 2.0; + path.poses[1].pose.position.y = 2.0; + path.poses[2].pose.position.x = -1.0; + path.poses[2].pose.position.y = -1.0; + ctrl->setPlan(path); + auto rtn = ctrl->findDirectionChangeWrapper(pose); + EXPECT_EQ(rtn, sqrt(5.0)); + + path.poses[2].pose.position.x = 3.0; + path.poses[2].pose.position.y = 3.0; + ctrl->setPlan(path); + rtn = ctrl->findDirectionChangeWrapper(pose); + EXPECT_EQ(rtn, std::numeric_limits::max()); +} + TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared(); diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 9eda37688f1..7d0f19d3997 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.0.6 + 1.0.7 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md new file mode 100644 index 00000000000..707e3722941 --- /dev/null +++ b/nav2_simple_commander/README.md @@ -0,0 +1,111 @@ +# Nav2 Simple (Python3) Commander + +## Overview + +The goal of this package is to provide a "navigation as a library" capability to Python3 users. We provide an API that handles all the ROS2-y and Action Server-y things for you such that you can focus on building an application leveraging the capabilities of Nav2. We also provide you with demos and examples of API usage to build common basic capabilities in autonomous mobile robotics. + +This was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) at [Samsung Research](https://www.sra.samsung.com/), with initial prototypes being prepared for the Keynote at the [2021 ROS Developers Day](https://www.theconstructsim.com/ros-developers-day-2021/) conference (code can be found [here](https://github.com/SteveMacenski/nav2_rosdevday_2021)). + +![](media/readme.gif) + +## API + +The methods provided by the basic navigator are shown below, with inputs and expected returns. If a server fails, it may throw an exception or return a `None` object, so please be sure to properly wrap your navigation calls in try/catch and check results for `None` type. + +| Robot Navigator Method | Description | +| --------------------------------- | -------------------------------------------------------------------------- | +| setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. | +| goThroughPoses(poses) | Requests the robot to drive through a set of poses (list of `PoseStamped`).| +| goToPose(pose) | Requests the robot to drive to a pose (`PoseStamped`). | +| followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. | +| cancelNav() | Cancel an ongoing `goThroughPoses` `goToPose` or `followWaypoints` request.| +| isNavComplete() | Checks if navigation is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | +| getFeedback() | Gets feedback from navigation task, returns action server feedback object. | +| getResult() | Gets final result of navigation task, to be called after `isNavComplete` returns `True`. Returns action server result object. | +| getPath(start, goal) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | +| getPathThroughPoses(start, goals) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | +| changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. | +| clearAllCostmaps() | Clears both the global and local costmaps. | +| clearLocalCostmap() | Clears the local costmap. | +| clearGlobalCostmap() | Clears the global costmap. | +| getGlobalCostmap() | Returns the global costmap, `nav2_msgs/Costmap` | +| getLocalCostmap() | Returns the local costmap, `nav2_msgs/Costmap` | +| waitUntilNav2Active() | Blocks until Nav2 is completely online and lifecycle nodes are in the active state. To be used in conjunction with autostart or external lifecycle bringup. | +| lifecycleStartup() | Sends a request to all lifecycle management servers to bring them into the active state, to be used if autostart is `false` and you want this program to control Nav2's lifecycle. | +| lifecycleShutdown() | Sends a request to all lifecycle management servers to shut them down. | + +A general template for building applications is as follows: + +``` python3 + +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy + +rclpy.init() + +nav = BasicNavigator() +... +nav.setInitialPose(init_pose) +nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()` +... +nav.goToPose(goal_pose) +while not nav.isNavComplete(): + feedback = nav.getFeedback() + if feedback.navigation_duration > 600: + nav.cancelNav() +... +result = nav.getResult() +if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') +elif result == NavigationResult.CANCELED: + print('Goal was canceled!') +elif result == NavigationResult.FAILED: + print('Goal failed!') +``` + +## Usage of Demos and Examples + +Make sure to install the `aws_robomaker_small_warehouse_world` package or build it in your local workspace alongside Nav2. It can be found [here](https://github.com/aws-robotics/aws-robomaker-small-warehouse-world). The demonstrations, examples, and launch files assume you're working with this gazebo world (such that the hard-programmed shelf locations and routes highlighting the API are meaningful). + +Make sure you have set the model directory of turtlebot3 simulation and aws warehouse world to the `GAZEBO_MODEL_PATH`. There are 2 main ways to run the demos of the `nav2_simple_commander` API. + +### Automatically + +The main benefit of this is automatically showing the above demonstrations in a single command for the default robot model and world. This will make use of Nav2's default robot and parameters set out in the main simulation launch file in `nav2_bringup`. + +``` bash +# Launch the launch file for the demo / example +ros2 launch nav2_simple_commander demo_security_launch.py +``` + +This will bring up the robot in the AWS Warehouse in a reasonable position, launch the autonomy script, and complete some task to demonstrate the `nav2_simple_commander` API. + +### Manually + +The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstation. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment. + +``` bash +# Terminal 1: launch your robot navigation and simulation (or physical robot). For example +ros2 launch nav2_bringup tb3_simulation_launch.py world:=/path/to/aws_robomaker_small_warehouse_world/.world map:=/path/to/aws_robomaker_small_warehouse_world/.yaml + +# Terminal 2: launch your autonomy / application demo or example. For example +ros2 run nav2_simple_commander demo_security +``` + +Then you should see the autonomy application running! + +## Examples + +The `nav2_simple_commander` has a few examples to highlight the API functions available to you as a user: + +- `example_nav_to_pose.py` - Demonstrates the navigate to pose capabilities of the navigator, as well as a number of auxiliary methods. +- `example_nav_through_poses.py` - Demonstrates the navigate through poses capabilities of the navigator, as well as a number of auxiliary methods. +- `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods required. + +## Demos + +The `nav2_simple_commander` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the `nav2_simple_commander` API: + +- `demo_security.py` - A simple security robot application, showing how to have a robot follow a security route using Navigate Through Poses to do a patrol route, indefinitely. +- `demo_picking.py` - A simple item picking application, showing how to have a robot drive to a specific shelf in a warehouse to either pick an item or have a person place an item into a basket and deliver it to a destination for shipping using Navigate To Pose. +- `demo_inspection.py` - A simple shelf inspection application, showing how to use the Waypoint Follower and task executors to take pictures, RFID scans, etc of shelves to analyze the current shelf statuses and locate items in the warehouse. diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py new file mode 100644 index 00000000000..3cfbd1a2c3c --- /dev/null +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_inspection', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/nav_through_poses_example_launch.py b/nav2_simple_commander/launch/nav_through_poses_example_launch.py new file mode 100644 index 00000000000..11f39b9a37c --- /dev/null +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_nav_through_poses', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/nav_to_pose_example_launch.py new file mode 100644 index 00000000000..7d019884fe6 --- /dev/null +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_nav_to_pose', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py new file mode 100644 index 00000000000..5978570243f --- /dev/null +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_picking', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py new file mode 100644 index 00000000000..55ef0fe65a2 --- /dev/null +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_security', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/warehouse.world b/nav2_simple_commander/launch/warehouse.world new file mode 100644 index 00000000000..639965a031d --- /dev/null +++ b/nav2_simple_commander/launch/warehouse.world @@ -0,0 +1,682 @@ + + + + + 0 0 -9.8 + + 0.001 + 1 + 1000 + + + + + + + + model://aws_robomaker_warehouse_ShelfF_01 + + -5.795143 -0.956635 0 0 0 0 + + + + + model://aws_robomaker_warehouse_WallB_01 + + 0.0 0.0 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 0.57943 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -4.827049 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -8.6651 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -1.242668 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -3.038551 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -6.750542 0 0 0 0 + + + + + + + model://aws_robomaker_warehouse_GroundB_01 + + 0.0 0.0 -0.090092 0 0 0 + + + + + model://aws_robomaker_warehouse_Lamp_01 + + 0 0 -4 0 0 0 + + + + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 9.631706 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_Bucket_01 + + -1.8321 -6.3752 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 8.59 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 5.708138 8.616844 -0.017477 0 0 0 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 3.408638 8.616844 -0.017477 0 0 0 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + -1.491287 5.222435 -0.017477 0 0 -1.583185 + + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.324959 3.822449 -0.012064 0 0 1.563871 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.54171 3.816475 -0.015663 0 0 -1.583191 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.384239 6.137154 0 0 0 3.150000 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.236 6.137154 0 0 0 3.150000 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.573677 2.301994 -0.015663 0 0 -3.133191 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.2196 9.407 -0.015663 0 0 1.563871 + + + + + model://aws_robomaker_warehouse_ClutteringD_01 + + -1.634682 -7.811813 -0.319559 0 0 0 + + + + + model://aws_robomaker_warehouse_TrashCanC_01 + + -1.592441 7.715420 0 0 0 0 + + + + + + model://aws_robomaker_warehouse_PalletJackB_01 + + -0.276098 -9.481944 0.023266 0 0 0 + + + + + 0 0 9 0 0 0 + 0.5 0.5 0.5 1 + 0.2 0.2 0.2 1 + + 80 + 0.3 + 0.01 + 0.001 + + 1 + 0.1 0.1 -1 + + + + + -4.70385 10.895 16.2659 -0 0.921795 -1.12701 + orbit + perspective + + + + + 3.45 2.15 0.01 0.0 0.0 3.14 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + + + + + true + 200 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + false + + + ~/out:=imu + + + + + + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.052 0 0.111 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.064 0 0.121 0 0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + + true + true + -0.064 0 0.121 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + + + + + + 30 + + + wheel_left_joint + wheel_right_joint + + + 0.287 + 0.066 + + + 20 + 1.0 + + cmd_vel + + + true + true + false + + odom + odom + base_footprint + + + + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + + + diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py new file mode 100644 index 00000000000..eddcc0d4b5d --- /dev/null +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_waypoint_follower', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/media/readme.gif b/nav2_simple_commander/media/readme.gif new file mode 100644 index 00000000000..d3a925a252c Binary files /dev/null and b/nav2_simple_commander/media/readme.gif differ diff --git a/nav2_simple_commander/nav2_simple_commander/__init__.py b/nav2_simple_commander/nav2_simple_commander/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py new file mode 100644 index 00000000000..111e54dc9d3 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -0,0 +1,101 @@ +#! /usr/bin/env python3 +# Copyright 2021 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. + +from copy import deepcopy + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy + +""" +Basic stock inspection demo. In this demonstration, the expectation +is that there are cameras or RFID sensors mounted on the robots +collecting information about stock quantity and location. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Inspection route, probably read in from a file for a real application + # from either a map or drive and repeat. + inspection_route = [ + [3.461, -0.450], + [5.531, -0.450], + [3.461, -2.200], + [5.531, -2.200], + [3.661, -4.121], + [5.431, -4.121], + [3.661, -5.850], + [5.431, -5.800]] + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + # Send our route + inspection_points = [] + inspection_pose = PoseStamped() + inspection_pose.header.frame_id = 'map' + inspection_pose.header.stamp = navigator.get_clock().now().to_msg() + inspection_pose.pose.orientation.z = 1.0 + inspection_pose.pose.orientation.w = 0.0 + for pt in inspection_route: + inspection_pose.pose.position.x = pt[0] + inspection_pose.pose.position.y = pt[1] + inspection_points.append(deepcopy(inspection_pose)) + navigator.followWaypoints(inspection_points) + + # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) + # Simply the current waypoint ID for the demonstation + i = 0 + while not navigator.isNavComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points))) + + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Inspection of shelves complete! Returning to start...') + elif result == NavigationResult.CANCELED: + print('Inspection of shelving was canceled. Returning to start...') + exit(1) + elif result == NavigationResult.FAILED: + print('Inspection of shelving failed! Returning to start...') + + # go back to start + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + navigator.goToPose(initial_pose) + while not navigator.isNavComplete(): + pass + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py new file mode 100644 index 00000000000..8cfcb2aec34 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -0,0 +1,123 @@ +#! /usr/bin/env python3 +# Copyright 2021 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. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult + +import rclpy +from rclpy.duration import Duration + + +# Shelf positions for picking +shelf_positions = { + 'shelf_A': [-3.829, -7.604], + 'shelf_B': [-3.791, -3.287], + 'shelf_C': [-3.791, 1.254], + 'shelf_D': [-3.24, 5.861]} + +# Shipping destination for picked products +shipping_destinations = { + 'recycling': [-0.205, 7.403], + 'pallet_jack7': [-0.073, -8.497], + 'conveyer_432': [6.217, 2.153], + 'frieght_bay_3': [-6.349, 9.147]} + +""" +Basic item picking demo. In this demonstration, the expectation +is that there is a person at the item shelf to put the item on the robot +and at the pallet jack to remove it +(probably with some kind of button for 'got item, robot go do next task'). +""" + + +def main(): + # Recieved virtual request for picking item at Shelf A and bring to + # worker at the pallet jack 7 for shipping. This request would + # contain the shelf ID ("shelf_A") and shipping destination ("pallet_jack7") + #################### + request_item_location = 'shelf_C' + request_destination = 'pallet_jack7' + #################### + + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + shelf_item_pose = PoseStamped() + shelf_item_pose.header.frame_id = 'map' + shelf_item_pose.header.stamp = navigator.get_clock().now().to_msg() + shelf_item_pose.pose.position.x = shelf_positions[request_item_location][0] + shelf_item_pose.pose.position.y = shelf_positions[request_item_location][1] + shelf_item_pose.pose.orientation.z = 1.0 + shelf_item_pose.pose.orientation.w = 0.0 + print(f'Received request for item picking at {request_item_location}.') + navigator.goToPose(shelf_item_pose) + + # Do something during our route + # (e.x. queue up future tasks or detect person for fine-tuned positioning) + # Simply print information for workers on the robot's ETA for the demonstation + i = 0 + while not navigator.isNavComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time of arrival at ' + request_item_location + + ' for worker: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Got product from ' + request_item_location + + '! Bringing product to shipping destination (' + request_destination + ')...') + shipping_destination = PoseStamped() + shipping_destination.header.frame_id = 'map' + shipping_destination.header.stamp = navigator.get_clock().now().to_msg() + shipping_destination.pose.position.x = shipping_destinations[request_destination][0] + shipping_destination.pose.position.y = shipping_destinations[request_destination][1] + shipping_destination.pose.orientation.z = 1.0 + shipping_destination.pose.orientation.w = 0.0 + navigator.goToPose(shipping_destination) + + elif result == NavigationResult.CANCELED: + print(f'Task at {request_item_location} was canceled. Returning to staging point...') + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + navigator.goToPose(initial_pose) + + elif result == NavigationResult.FAILED: + print(f'Task at {request_item_location} failed!') + exit(-1) + + while not navigator.isNavComplete(): + pass + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py new file mode 100644 index 00000000000..8180bc36b4d --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -0,0 +1,107 @@ +#! /usr/bin/env python3 +# Copyright 2021 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. + +from copy import deepcopy + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult + +import rclpy +from rclpy.duration import Duration + + +""" +Basic security route patrol demo. In this demonstration, the expectation +is that there are security cameras mounted on the robots recording or being +watched live by security staff. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Security route, probably read in from a file for a real application + # from either a map or drive and repeat. + security_route = [ + [1.792, 2.144], + [1.792, -5.44], + [1.792, -9.427], + [-3.665, -9.427], + [-3.665, -4.303], + [-3.665, 2.330], + [-3.665, 9.283]] + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + # Do security route until dead + while rclpy.ok(): + # Send our route + route_poses = [] + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = navigator.get_clock().now().to_msg() + pose.pose.orientation.w = 1.0 + for pt in security_route: + pose.pose.position.x = pt[0] + pose.pose.position.y = pt[1] + route_poses.append(deepcopy(pose)) + navigator.goThroughPoses(route_poses) + + # Do something during our route (e.x. AI detection on camera images for anomalies) + # Simply print ETA for the demonstation + i = 0 + while not navigator.isNavComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time to complete current route: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + # Some failure mode, must stop since the robot is clearly stuck + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0): + print('Navigation has exceeded timeout of 180s, canceling request.') + navigator.cancelNav() + + # If at end of route, reverse the route to restart + security_route.reverse() + + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Route complete! Restarting...') + elif result == NavigationResult.CANCELED: + print('Security route was canceled, exiting.') + exit(1) + elif result == NavigationResult.FAILED: + print('Security route failed! Restarting from other side...') + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py new file mode 100644 index 00000000000..f4523b8974f --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -0,0 +1,139 @@ +#! /usr/bin/env python3 +# Copyright 2021 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. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy +from rclpy.duration import Duration + +""" +Basic navigation demo to go to poses. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # set our demo's goal poses + goal_poses = [] + goal_pose1 = PoseStamped() + goal_pose1.header.frame_id = 'map' + goal_pose1.header.stamp = navigator.get_clock().now().to_msg() + goal_pose1.pose.position.x = 1.5 + goal_pose1.pose.position.y = 0.55 + goal_pose1.pose.orientation.w = 0.707 + goal_pose1.pose.orientation.z = 0.707 + goal_poses.append(goal_pose1) + + # additional goals can be appended + goal_pose2 = PoseStamped() + goal_pose2.header.frame_id = 'map' + goal_pose2.header.stamp = navigator.get_clock().now().to_msg() + goal_pose2.pose.position.x = 1.5 + goal_pose2.pose.position.y = -3.75 + goal_pose2.pose.orientation.w = 0.707 + goal_pose2.pose.orientation.z = 0.707 + goal_poses.append(goal_pose2) + goal_pose3 = PoseStamped() + goal_pose3.header.frame_id = 'map' + goal_pose3.header.stamp = navigator.get_clock().now().to_msg() + goal_pose3.pose.position.x = -3.6 + goal_pose3.pose.position.y = -4.75 + goal_pose3.pose.orientation.w = 0.707 + goal_pose3.pose.orientation.z = 0.707 + goal_poses.append(goal_pose3) + + # sanity check a valid path exists + # path = navigator.getPathThroughPoses(initial_pose, goal_poses) + + navigator.goThroughPoses(goal_poses) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time of arrival: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + # Some navigation timeout to demo cancellation + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): + navigator.cancelNav() + + # Some navigation request change to demo preemption + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=35.0): + goal_pose4 = PoseStamped() + goal_pose4.header.frame_id = 'map' + goal_pose4.header.stamp = navigator.get_clock().now().to_msg() + goal_pose4.pose.position.x = -5.0 + goal_pose4.pose.position.y = -4.75 + goal_pose4.pose.orientation.w = 0.707 + goal_pose4.pose.orientation.z = 0.707 + navigator.goThroughPoses([goal_pose4]) + + # Do something depending on the return code + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') + elif result == NavigationResult.CANCELED: + print('Goal was canceled!') + elif result == NavigationResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py new file mode 100644 index 00000000000..cd302e418c9 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -0,0 +1,112 @@ +#! /usr/bin/env python3 +# Copyright 2021 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. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy +from rclpy.duration import Duration + +""" +Basic navigation demo to go to pose. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # Go to our demos first goal pose + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = -2.0 + goal_pose.pose.position.y = -0.5 + goal_pose.pose.orientation.w = 1.0 + + # sanity check a valid path exists + # path = navigator.getPath(initial_pose, goal_pose) + + navigator.goToPose(goal_pose) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time of arrival: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + # Some navigation timeout to demo cancellation + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): + navigator.cancelNav() + + # Some navigation request change to demo preemption + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0): + goal_pose.pose.position.x = -3.0 + navigator.goToPose(goal_pose) + + # Do something depending on the return code + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') + elif result == NavigationResult.CANCELED: + print('Goal was canceled!') + elif result == NavigationResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py new file mode 100644 index 00000000000..8ed0fc3d16b --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -0,0 +1,142 @@ +#! /usr/bin/env python3 +# Copyright 2021 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. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy +from rclpy.duration import Duration + +""" +Basic navigation demo to go to poses. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # set our demo's goal poses to follow + goal_poses = [] + goal_pose1 = PoseStamped() + goal_pose1.header.frame_id = 'map' + goal_pose1.header.stamp = navigator.get_clock().now().to_msg() + goal_pose1.pose.position.x = 1.5 + goal_pose1.pose.position.y = 0.55 + goal_pose1.pose.orientation.w = 0.707 + goal_pose1.pose.orientation.z = 0.707 + goal_poses.append(goal_pose1) + + # additional goals can be appended + goal_pose2 = PoseStamped() + goal_pose2.header.frame_id = 'map' + goal_pose2.header.stamp = navigator.get_clock().now().to_msg() + goal_pose2.pose.position.x = 1.5 + goal_pose2.pose.position.y = -3.75 + goal_pose2.pose.orientation.w = 0.707 + goal_pose2.pose.orientation.z = 0.707 + goal_poses.append(goal_pose2) + goal_pose3 = PoseStamped() + goal_pose3.header.frame_id = 'map' + goal_pose3.header.stamp = navigator.get_clock().now().to_msg() + goal_pose3.pose.position.x = -3.6 + goal_pose3.pose.position.y = -4.75 + goal_pose3.pose.orientation.w = 0.707 + goal_pose3.pose.orientation.z = 0.707 + goal_poses.append(goal_pose3) + + # sanity check a valid path exists + # path = navigator.getPath(initial_pose, goal_pose1) + + nav_start = navigator.get_clock().now() + navigator.followWaypoints(goal_poses) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses))) + now = navigator.get_clock().now() + + # Some navigation timeout to demo cancellation + if now - nav_start > Duration(seconds=600.0): + navigator.cancelNav() + + # Some follow waypoints request change to demo preemption + if now - nav_start > Duration(seconds=35.0): + goal_pose4 = PoseStamped() + goal_pose4.header.frame_id = 'map' + goal_pose4.header.stamp = now.to_msg() + goal_pose4.pose.position.x = -5.0 + goal_pose4.pose.position.y = -4.75 + goal_pose4.pose.orientation.w = 0.707 + goal_pose4.pose.orientation.z = 0.707 + goal_poses = [goal_pose4] + nav_start = now + navigator.followWaypoints(goal_poses) + + # Do something depending on the return code + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') + elif result == NavigationResult.CANCELED: + print('Goal was canceled!') + elif result == NavigationResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py new file mode 100644 index 00000000000..80c21cfb580 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -0,0 +1,421 @@ +#! /usr/bin/env python3 +# Copyright 2021 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. + + +from enum import Enum +import time + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose +from nav2_msgs.action import FollowWaypoints, NavigateThroughPoses, NavigateToPose +from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy +from rclpy.qos import QoSProfile, QoSReliabilityPolicy + + +class NavigationResult(Enum): + UKNOWN = 0 + SUCCEEDED = 1 + CANCELED = 2 + FAILED = 3 + + +class BasicNavigator(Node): + + def __init__(self): + super().__init__(node_name='basic_navigator') + self.initial_pose = PoseStamped() + self.initial_pose.header.frame_id = 'map' + self.goal_handle = None + self.result_future = None + self.feedback = None + self.status = None + + amcl_pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1) + + self.initial_pose_received = False + self.nav_through_poses_client = ActionClient(self, + NavigateThroughPoses, + 'navigate_through_poses') + self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') + self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, + 'compute_path_to_pose') + self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses, + 'compute_path_through_poses') + self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped, + 'amcl_pose', + self._amclPoseCallback, + amcl_pose_qos) + self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, + 'initialpose', + 10) + self.change_maps_srv = self.create_client(LoadMap, '/map_server/load_map') + self.clear_costmap_global_srv = self.create_client( + ClearEntireCostmap, '/global_costmap/clear_entirely_global_costmap') + self.clear_costmap_local_srv = self.create_client( + ClearEntireCostmap, '/local_costmap/clear_entirely_local_costmap') + self.get_costmap_global_srv = self.create_client(GetCostmap, '/global_costmap/get_costmap') + self.get_costmap_local_srv = self.create_client(GetCostmap, '/local_costmap/get_costmap') + + def setInitialPose(self, initial_pose): + """Set the initial pose to the localization system.""" + self.initial_pose_received = False + self.initial_pose = initial_pose + self._setInitialPose() + + def goThroughPoses(self, poses): + """Send a `NavThroughPoses` action request.""" + self.debug("Waiting for 'NavigateThroughPoses' action server") + while not self.nav_through_poses_client.wait_for_server(timeout_sec=1.0): + self.info("'NavigateThroughPoses' action server not available, waiting...") + + goal_msg = NavigateThroughPoses.Goal() + goal_msg.poses = poses + + self.info(f'Navigating with {len(goal_msg.poses)} goals....') + send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error(f'Goal with {len(poses)} poses was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def goToPose(self, pose): + """Send a `NavToPose` action request.""" + self.debug("Waiting for 'NavigateToPose' action server") + while not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0): + self.info("'NavigateToPose' action server not available, waiting...") + + goal_msg = NavigateToPose.Goal() + goal_msg.pose = pose + + self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' + + str(pose.pose.position.y) + '...') + send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Goal to ' + str(pose.pose.position.x) + ' ' + + str(pose.pose.position.y) + ' was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def followWaypoints(self, poses): + """Send a `FollowWaypoints` action request.""" + self.debug("Waiting for 'FollowWaypoints' action server") + while not self.follow_waypoints_client.wait_for_server(timeout_sec=1.0): + self.info("'FollowWaypoints' action server not available, waiting...") + + goal_msg = FollowWaypoints.Goal() + goal_msg.poses = poses + + self.info(f'Following {len(goal_msg.poses)} goals....') + send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error(f'Following {len(poses)} waypoints request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def cancelNav(self): + """Cancel pending navigation request of any type.""" + self.info('Canceling current goal.') + if self.result_future: + future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, future) + return + + def isNavComplete(self): + """Check if the navigation request of any type is complete yet.""" + if not self.result_future: + # task was cancelled or completed + return True + rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10) + if self.result_future.result(): + self.status = self.result_future.result().status + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.debug(f'Goal with failed with status code: {self.status}') + return True + else: + # Timed out, still processing, not complete yet + return False + + self.debug('Goal succeeded!') + return True + + def getFeedback(self): + """Get the pending action feedback message.""" + return self.feedback + + def getResult(self): + """Get the pending action result message.""" + if self.status == GoalStatus.STATUS_SUCCEEDED: + return NavigationResult.SUCCEEDED + elif self.status == GoalStatus.STATUS_ABORTED: + return NavigationResult.FAILED + elif self.status == GoalStatus.STATUS_CANCELED: + return NavigationResult.CANCELED + else: + return NavigationResult.UNKNOWN + + def waitUntilNav2Active(self): + """Block until the full navigation system is up and running.""" + self._waitForNodeToActivate('amcl') + self._waitForInitialPose() + self._waitForNodeToActivate('bt_navigator') + self.info('Nav2 is ready for use!') + return + + def getPath(self, start, goal): + """Send a `ComputePathToPose` action request.""" + self.debug("Waiting for 'ComputePathToPose' action server") + while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputePathToPose' action server not available, waiting...") + + goal_msg = ComputePathToPose.Goal() + goal_msg.goal = goal + goal_msg.start = start + + self.info('Getting path...') + send_goal_future = self.compute_path_to_pose_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Get path was rejected!') + return None + + self.result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, self.result_future) + self.status = self.result_future.result().status + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn(f'Getting path failed with status code: {self.status}') + return None + + return self.result_future.result().result.path + + def getPathThroughPoses(self, start, goals): + """Send a `ComputePathThroughPoses` action request.""" + self.debug("Waiting for 'ComputePathThroughPoses' action server") + while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputePathThroughPoses' action server not available, waiting...") + + goal_msg = ComputePathThroughPoses.Goal() + goal_msg.goals = goals + goal_msg.start = start + + self.info('Getting path...') + send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Get path was rejected!') + return None + + self.result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, self.result_future) + self.status = self.result_future.result().status + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn(f'Getting path failed with status code: {self.status}') + return None + + return self.result_future.result().result.path + + def changeMap(self, map_filepath): + """Change the current static map in the map server.""" + while not self.change_maps_srv.wait_for_service(timeout_sec=1.0): + self.info('change map service not available, waiting...') + req = LoadMap.Request() + req.map_url = map_filepath + future = self.change_maps_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + status = future.result().result + if status != LoadMap.Response().RESULT_SUCCESS: + self.error('Change map request failed!') + else: + self.info('Change map request was successful!') + return + + def clearAllCostmaps(self): + """Clear all costmaps.""" + self.clearLocalCostmap() + self.clearGlobalCostmap() + return + + def clearLocalCostmap(self): + """Clear local costmap.""" + while not self.clear_costmap_local_srv.wait_for_service(timeout_sec=1.0): + self.info('Clear local costmaps service not available, waiting...') + req = ClearEntireCostmap.Request() + future = self.clear_costmap_local_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return + + def clearGlobalCostmap(self): + """Clear global costmap.""" + while not self.clear_costmap_global_srv.wait_for_service(timeout_sec=1.0): + self.info('Clear global costmaps service not available, waiting...') + req = ClearEntireCostmap.Request() + future = self.clear_costmap_global_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return + + def getGlobalCostmap(self): + """Get the global costmap.""" + while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0): + self.info('Get global costmaps service not available, waiting...') + req = GetCostmap.Request() + future = self.get_costmap_global_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result().map + + def getLocalCostmap(self): + """Get the local costmap.""" + while not self.get_costmap_local_srv.wait_for_service(timeout_sec=1.0): + self.info('Get local costmaps service not available, waiting...') + req = GetCostmap.Request() + future = self.get_costmap_local_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result().map + + def lifecycleStartup(self): + """Startup nav2 lifecycle system.""" + self.info('Starting up lifecycle nodes based on lifecycle_manager.') + for srv_name, srv_type in self.get_service_names_and_types(): + if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes': + self.info(f'Starting up {srv_name}') + mgr_client = self.create_client(ManageLifecycleNodes, srv_name) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info(f'{srv_name} service not available, waiting...') + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().STARTUP + future = mgr_client.call_async(req) + + # starting up requires a full map->odom->base_link TF tree + # so if we're not successful, try forwarding the initial pose + while True: + rclpy.spin_until_future_complete(self, future, timeout_sec=0.10) + if not future: + self._waitForInitialPose() + else: + break + self.info('Nav2 is ready for use!') + return + + def lifecycleShutdown(self): + """Shutdown nav2 lifecycle system.""" + self.info('Shutting down lifecycle nodes based on lifecycle_manager.') + for srv_name, srv_type in self.get_service_names_and_types(): + if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes': + self.info(f'Shutting down {srv_name}') + mgr_client = self.create_client(ManageLifecycleNodes, srv_name) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info(f'{srv_name} service not available, waiting...') + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + future.result() + return + + def _waitForNodeToActivate(self, node_name): + # Waits for the node within the tester namespace to become active + self.debug(f'Waiting for {node_name} to become active..') + node_service = f'{node_name}/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info(f'{node_service} service not available, waiting...') + + req = GetState.Request() + state = 'unknown' + while state != 'active': + self.debug(f'Getting {node_name} state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.debug(f'Result of get_state: {state}') + time.sleep(2) + return + + def _waitForInitialPose(self): + while not self.initial_pose_received: + self.info('Setting initial pose') + self._setInitialPose() + self.info('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1.0) + return + + def _amclPoseCallback(self, msg): + self.debug('Received amcl pose') + self.initial_pose_received = True + return + + def _feedbackCallback(self, msg): + self.debug('Received action feedback message') + self.feedback = msg.feedback + return + + def _setInitialPose(self): + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose.pose + msg.header.frame_id = self.initial_pose.header.frame_id + msg.header.stamp = self.initial_pose.header.stamp + self.info('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + return + + def info(self, msg): + self.get_logger().info(msg) + return + + def warn(self, msg): + self.get_logger().warn(msg) + return + + def error(self, msg): + self.get_logger().error(msg) + return + + def debug(self, msg): + self.get_logger().debug(msg) + return diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml new file mode 100644 index 00000000000..f63a5aa4e2c --- /dev/null +++ b/nav2_simple_commander/package.xml @@ -0,0 +1,25 @@ + + + + nav2_simple_commander + 1.0.7 + An importable library for writing mobile robot applications in python3 + steve + Apache-2.0 + + python-enum34 + rclpy + geometry_msgs + nav2_msgs + action_msgs + lifecycle_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/nav2_simple_commander/resource/nav2_simple_commander b/nav2_simple_commander/resource/nav2_simple_commander new file mode 100644 index 00000000000..e69de29bb2d diff --git a/nav2_simple_commander/setup.cfg b/nav2_simple_commander/setup.cfg new file mode 100644 index 00000000000..2d91d903923 --- /dev/null +++ b/nav2_simple_commander/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/nav2_simple_commander +[install] +install-scripts=$base/lib/nav2_simple_commander diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py new file mode 100644 index 00000000000..ca44856c5ad --- /dev/null +++ b/nav2_simple_commander/setup.py @@ -0,0 +1,36 @@ +from glob import glob +import os + +from setuptools import setup + + +package_name = 'nav2_simple_commander' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='steve', + maintainer_email='stevenmacenski@gmail.com', + description='An importable library for writing mobile robot applications in python3', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_nav_to_pose = nav2_simple_commander.example_nav_to_pose:main', + 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', + 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', + 'demo_picking = nav2_simple_commander.demo_picking:main', + 'demo_inspection = nav2_simple_commander.demo_inspection:main', + 'demo_security = nav2_simple_commander.demo_security:main', + ], + }, +) diff --git a/nav2_simple_commander/test/test_copyright.py b/nav2_simple_commander/test/test_copyright.py new file mode 100644 index 00000000000..cc8ff03f79a --- /dev/null +++ b/nav2_simple_commander/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/nav2_simple_commander/test/test_flake8.py b/nav2_simple_commander/test/test_flake8.py new file mode 100644 index 00000000000..27ee1078ff0 --- /dev/null +++ b/nav2_simple_commander/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/nav2_simple_commander/test/test_pep257.py b/nav2_simple_commander/test/test_pep257.py new file mode 100644 index 00000000000..b234a3840f4 --- /dev/null +++ b/nav2_simple_commander/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index ff235b02cfd..8fc0f5d1306 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/smoother.hpp" @@ -87,6 +88,12 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner const geometry_msgs::msg::PoseStamped & goal) override; protected: + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + std::unique_ptr> _a_star; GridCollisionChecker _collision_checker; std::unique_ptr _smoother; @@ -100,6 +107,19 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner bool _downsample_costmap; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; + bool _allow_unknown; + int _max_iterations; + int _max_on_approach_iterations; + bool _use_final_approach_orientation; + SearchInfo _search_info; + std::string _motion_model_for_search; + MotionModel _motion_model; + std::mutex _mutex; + rclcpp_lifecycle::LifecycleNode::WeakPtr _node; + + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr _parameters_client; + rclcpp::Subscription::SharedPtr _parameter_event_sub; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 109913b7fd0..4fd56731aca 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -87,21 +87,41 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner const geometry_msgs::msg::PoseStamped & goal) override; protected: + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + std::unique_ptr> _a_star; GridCollisionChecker _collision_checker; std::unique_ptr _smoother; rclcpp::Clock::SharedPtr _clock; rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")}; nav2_costmap_2d::Costmap2D * _costmap; + std::shared_ptr _costmap_ros; std::unique_ptr _costmap_downsampler; std::string _global_frame, _name; + float _lookup_table_dim; float _tolerance; + bool _downsample_costmap; int _downsampling_factor; - unsigned int _angle_quantizations; double _angle_bin_size; - bool _downsample_costmap; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + unsigned int _angle_quantizations; + bool _allow_unknown; + int _max_iterations; + SearchInfo _search_info; double _max_planning_time; + double _lookup_table_size; + std::string _motion_model_for_search; + MotionModel _motion_model; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + std::mutex _mutex; + rclcpp_lifecycle::LifecycleNode::WeakPtr _node; + + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr _parameters_client; + rclcpp::Subscription::SharedPtr _parameter_event_sub; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index af2c8c69525..99f68055d8b 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.0.6 + 1.0.7 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index cd09cc79432..11d1976dae3 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -329,7 +329,7 @@ bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path current_node = current_node->parent; } - return path.size() > 1; + return path.size() > 0; } template @@ -346,7 +346,7 @@ bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path) current_node = current_node->parent; } - return path.size() > 1; + return path.size() > 0; } template @@ -557,7 +557,9 @@ typename AStarAlgorithm::AnalyticExpansionNodes AStarAlgorithm::ge unsigned int num_intervals = std::floor(d / sqrt_2); AnalyticExpansionNodes possible_nodes; - possible_nodes.reserve(num_intervals - 1); // We won't store this node or the goal + // When "from" and "to" are zero or one cell away, + // num_intervals == 0 + possible_nodes.reserve(num_intervals); // We won't store this node or the goal std::vector reals; // Pre-allocate @@ -573,12 +575,12 @@ typename AStarAlgorithm::AnalyticExpansionNodes AStarAlgorithm::ge node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); reals = s.reals(); angle = reals[2] / node->motion_table.bin_size; - while (angle >= node->motion_table.num_angle_quantization_float) { - angle -= node->motion_table.num_angle_quantization_float; - } while (angle < 0.0) { angle += node->motion_table.num_angle_quantization_float; } + while (angle >= node->motion_table.num_angle_quantization_float) { + angle -= node->motion_table.num_angle_quantization_float; + } // Turn the pose into a node, and check if it is valid index = NodeT::getIndex( static_cast(reals[0]), diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 059f9f5445f..b02d99f326c 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -230,14 +230,14 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) const float & node_heading = node->pose.theta; float new_heading = node_heading + motion_model._theta; - if (new_heading >= num_angle_quantization_float) { - new_heading -= num_angle_quantization_float; - } - if (new_heading < 0.0) { new_heading += num_angle_quantization_float; } + if (new_heading >= num_angle_quantization_float) { + new_heading -= num_angle_quantization_float; + } + projection_list.emplace_back( delta_xs[i][node_heading] + node->pose.x, delta_ys[i][node_heading] + node->pose.y, diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index dfd295bcdfc..bf1d78721cc 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -19,12 +19,15 @@ #include #include "nav2_smac_planner/smac_planner_2d.hpp" +#include "nav2_util/geometry_utils.hpp" // #define BENCHMARK_TESTING namespace nav2_smac_planner { using namespace std::chrono; // NOLINT +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; SmacPlanner2D::SmacPlanner2D() : _a_star(nullptr), @@ -47,6 +50,7 @@ void SmacPlanner2D::configure( std::string name, std::shared_ptr/*tf*/, std::shared_ptr costmap_ros) { + _node = parent; auto node = parent.lock(); _logger = node->get_logger(); _clock = node->get_clock(); @@ -54,12 +58,6 @@ void SmacPlanner2D::configure( _name = name; _global_frame = costmap_ros->getGlobalFrameID(); - bool allow_unknown; - int max_iterations; - int max_on_approach_iterations; - SearchInfo search_info; - std::string motion_model_for_search; - // General planner params nav2_util::declare_parameter_if_not_declared( node, name + ".tolerance", rclcpp::ParameterValue(0.125)); @@ -72,17 +70,20 @@ void SmacPlanner2D::configure( node->get_parameter(name + ".downsampling_factor", _downsampling_factor); nav2_util::declare_parameter_if_not_declared( node, name + ".cost_travel_multiplier", rclcpp::ParameterValue(2.0)); - node->get_parameter(name + ".cost_travel_multiplier", search_info.cost_penalty); + node->get_parameter(name + ".cost_travel_multiplier", _search_info.cost_penalty); nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); - node->get_parameter(name + ".allow_unknown", allow_unknown); + node->get_parameter(name + ".allow_unknown", _allow_unknown); nav2_util::declare_parameter_if_not_declared( node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); - node->get_parameter(name + ".max_iterations", max_iterations); + node->get_parameter(name + ".max_iterations", _max_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); - node->get_parameter(name + ".max_on_approach_iterations", max_on_approach_iterations); + node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".use_final_approach_orientation", _use_final_approach_orientation); nav2_util::declare_parameter_if_not_declared( node, name + ".max_planning_time", rclcpp::ParameterValue(1.0)); @@ -90,28 +91,28 @@ void SmacPlanner2D::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); - node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); - MotionModel motion_model = fromString(motion_model_for_search); - if (motion_model == MotionModel::UNKNOWN) { + node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search); + _motion_model = fromString(_motion_model_for_search); + if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( _logger, "Unable to get MotionModel search type. Given '%s', " "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", - motion_model_for_search.c_str()); + _motion_model_for_search.c_str()); } - if (max_on_approach_iterations <= 0) { + if (_max_on_approach_iterations <= 0) { RCLCPP_INFO( _logger, "On approach iteration selected as <= 0, " "disabling tolerance and on approach iterations."); - max_on_approach_iterations = std::numeric_limits::max(); + _max_on_approach_iterations = std::numeric_limits::max(); } - if (max_iterations <= 0) { + if (_max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " "disabling maximum iterations."); - max_iterations = std::numeric_limits::max(); + _max_iterations = std::numeric_limits::max(); } // Initialize collision checker @@ -122,11 +123,11 @@ void SmacPlanner2D::configure( 0.0 /*for 2D cost at inscribed isn't relevent*/); // Initialize A* template - _a_star = std::make_unique>(motion_model, search_info); + _a_star = std::make_unique>(_motion_model, _search_info); _a_star->initialize( - allow_unknown, - max_iterations, - max_on_approach_iterations, + _allow_unknown, + _max_iterations, + _max_on_approach_iterations, 0.0 /*unused for 2D*/, 1.0 /*unused for 2D*/); @@ -146,13 +147,23 @@ void SmacPlanner2D::configure( _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + // Setup callback for changes to parameters. + _parameters_client = std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + _parameter_event_sub = _parameters_client->on_parameter_event( + std::bind(&SmacPlanner2D::on_parameter_event_callback, this, _1)); + RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlanner2D with " "tolerance %.2f, maximum iterations %i, " "max on approach iterations %i, and %s. Using motion model: %s.", - _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, - allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(motion_model).c_str()); + _name.c_str(), _tolerance, _max_iterations, _max_on_approach_iterations, + _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(_motion_model).c_str()); } void SmacPlanner2D::activate() @@ -184,8 +195,10 @@ void SmacPlanner2D::cleanup() _name.c_str()); _a_star.reset(); _smoother.reset(); - _costmap_downsampler->on_cleanup(); - _costmap_downsampler.reset(); + if (_costmap_downsampler) { + _costmap_downsampler->on_cleanup(); + _costmap_downsampler.reset(); + } _raw_plan_publisher.reset(); } @@ -193,6 +206,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { + std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); std::unique_lock lock(*(_costmap->getMutex())); @@ -208,13 +222,13 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _a_star->setCollisionChecker(&_collision_checker); // Set starting point - unsigned int mx, my; - costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); - _a_star->setStart(mx, my, 0); + unsigned int mx_start, my_start, mx_goal, my_goal; + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); + _a_star->setStart(mx_start, my_start, 0); // Set goal point - costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); - _a_star->setGoal(mx, my, 0); + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + _a_star->setGoal(mx_goal, my_goal, 0); // Setup message nav_msgs::msg::Path plan; @@ -228,6 +242,23 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; + // Corner case of start and goal beeing on the same cell + if (mx_start == mx_goal && my_start == my_goal) { + if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_WARN(_logger, "Failed to create a unique pose path because of obstacles"); + return plan; + } + pose.pose = start.pose; + // if we have a different start and goal orientation, set the unique path pose to the goal + // orientation, unless use_final_approach_orientation=true where we need it to be the start + // orientation to avoid movement from the local planner + if (start.pose.orientation != goal.pose.orientation && !_use_final_approach_orientation) { + pose.pose.orientation = goal.pose.orientation; + } + plan.poses.push_back(pose); + return plan; + } + // Compute plan Node2D::CoordinateVector path; int num_iterations = 0; @@ -282,9 +313,129 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _smoother->smooth(plan, costmap, time_remaining); } + + // If use_final_approach_orientation=true, interpolate the last pose orientation from the + // previous pose to set the orientation to the 'final approach' orientation of the robot so + // it does not rotate. + // And deal with corner case of plan of length 1 + // If use_final_approach_orientation=false (default), override last pose orientation to match goal + size_t plan_size = plan.poses.size(); + if (_use_final_approach_orientation) { + if (plan_size == 1) { + plan.poses.back().pose.orientation = start.pose.orientation; + } else if (plan_size > 1) { + double dx, dy, theta; + auto last_pose = plan.poses.back().pose.position; + auto approach_pose = plan.poses[plan_size - 2].pose.position; + dx = last_pose.x - approach_pose.x; + dy = last_pose.y - approach_pose.y; + theta = atan2(dy, dx); + plan.poses.back().pose.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(theta); + } + } else if (plan_size > 0) { + plan.poses.back().pose.orientation = goal.pose.orientation; + } + return plan; } +void SmacPlanner2D::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + std::lock_guard lock_reinit(_mutex); + + bool reinit_a_star = false; + bool reinit_downsampler = false; + + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == _name + ".tolerance") { + _tolerance = static_cast(value.double_value); + } else if (name == _name + ".cost_travel_multiplier") { + reinit_a_star = true; + _search_info.cost_penalty = value.double_value; + } else if (name == _name + ".max_planning_time") { + _max_planning_time = value.double_value; + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == _name + ".downsample_costmap") { + reinit_downsampler = true; + _downsample_costmap = value.bool_value; + } else if (name == _name + ".allow_unknown") { + reinit_a_star = true; + _allow_unknown = value.bool_value; + } else if (name == _name + ".use_final_approach_orientation") { + _use_final_approach_orientation = value.bool_value; + } + } else if (type == ParameterType::PARAMETER_INTEGER) { + if (name == _name + ".downsampling_factor") { + reinit_downsampler = true; + _downsampling_factor = value.integer_value; + } else if (name == _name + ".max_iterations") { + reinit_a_star = true; + _max_iterations = value.integer_value; + if (_max_iterations <= 0) { + RCLCPP_INFO( + _logger, "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = value.integer_value; + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + } + } else if (type == ParameterType::PARAMETER_STRING) { + if (name == _name + ".motion_model_for_search") { + reinit_a_star = true; + _motion_model = fromString(value.string_value); + if (_motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + _motion_model_for_search.c_str()); + } + } + } + } + + // Re-init if needed with mutex lock (to avoid re-init while creating a plan) + if (reinit_a_star || reinit_downsampler) { + // Re-Initialize A* template + if (reinit_a_star) { + _a_star = std::make_unique>(_motion_model, _search_info); + _a_star->initialize( + _allow_unknown, + _max_iterations, + _max_on_approach_iterations, + 0.0 /*unused for 2D*/, + 1.0 /*unused for 2D*/); + } + + // Re-Initialize costmap downsampler + if (reinit_downsampler) { + if (_downsample_costmap && _downsampling_factor > 1) { + auto node = _node.lock(); + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); + } + } + } +} + } // namespace nav2_smac_planner #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 1581bfbedcb..d86f6d2afd7 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -27,6 +27,8 @@ namespace nav2_smac_planner { using namespace std::chrono; // NOLINT +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; SmacPlannerHybrid::SmacPlannerHybrid() : _a_star(nullptr), @@ -49,19 +51,16 @@ void SmacPlannerHybrid::configure( std::string name, std::shared_ptr/*tf*/, std::shared_ptr costmap_ros) { + _node = parent; auto node = parent.lock(); _logger = node->get_logger(); _clock = node->get_clock(); _costmap = costmap_ros->getCostmap(); + _costmap_ros = costmap_ros; _name = name; _global_frame = costmap_ros->getGlobalFrameID(); - bool allow_unknown; - int max_iterations; int angle_quantizations; - double lookup_table_size; - SearchInfo search_info; - std::string motion_model_for_search; // General planner params nav2_util::declare_parameter_if_not_declared( @@ -79,93 +78,93 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); - node->get_parameter(name + ".allow_unknown", allow_unknown); + node->get_parameter(name + ".allow_unknown", _allow_unknown); nav2_util::declare_parameter_if_not_declared( node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); - node->get_parameter(name + ".max_iterations", max_iterations); + node->get_parameter(name + ".max_iterations", _max_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); - node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); + node->get_parameter(name + ".minimum_turning_radius", _search_info.minimum_turning_radius); nav2_util::declare_parameter_if_not_declared( node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue(false)); - node->get_parameter(name + ".cache_obstacle_heuristic", search_info.cache_obstacle_heuristic); + node->get_parameter(name + ".cache_obstacle_heuristic", _search_info.cache_obstacle_heuristic); nav2_util::declare_parameter_if_not_declared( node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); - node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty); + node->get_parameter(name + ".reverse_penalty", _search_info.reverse_penalty); nav2_util::declare_parameter_if_not_declared( node, name + ".change_penalty", rclcpp::ParameterValue(0.15)); - node->get_parameter(name + ".change_penalty", search_info.change_penalty); + node->get_parameter(name + ".change_penalty", _search_info.change_penalty); nav2_util::declare_parameter_if_not_declared( node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.50)); - node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty); + node->get_parameter(name + ".non_straight_penalty", _search_info.non_straight_penalty); nav2_util::declare_parameter_if_not_declared( node, name + ".cost_penalty", rclcpp::ParameterValue(1.7)); - node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); + node->get_parameter(name + ".cost_penalty", _search_info.cost_penalty); nav2_util::declare_parameter_if_not_declared( node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5)); - node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); + node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio); nav2_util::declare_parameter_if_not_declared( node, name + ".max_planning_time", rclcpp::ParameterValue(5.0)); node->get_parameter(name + ".max_planning_time", _max_planning_time); nav2_util::declare_parameter_if_not_declared( node, name + ".lookup_table_size", rclcpp::ParameterValue(20.0)); - node->get_parameter(name + ".lookup_table_size", lookup_table_size); + node->get_parameter(name + ".lookup_table_size", _lookup_table_size); nav2_util::declare_parameter_if_not_declared( node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); - node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); - MotionModel motion_model = fromString(motion_model_for_search); - if (motion_model == MotionModel::UNKNOWN) { + node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search); + _motion_model = fromString(_motion_model_for_search); + if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( _logger, "Unable to get MotionModel search type. Given '%s', " "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.", - motion_model_for_search.c_str()); + _motion_model_for_search.c_str()); } - if (max_iterations <= 0) { + if (_max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " "disabling maximum iterations."); - max_iterations = std::numeric_limits::max(); + _max_iterations = std::numeric_limits::max(); } // convert to grid coordinates - const double minimum_turning_radius_global_coords = search_info.minimum_turning_radius; - search_info.minimum_turning_radius = - search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); - float lookup_table_dim = - static_cast(lookup_table_size) / + const double minimum_turning_radius_global_coords = _search_info.minimum_turning_radius; + _search_info.minimum_turning_radius = + _search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); + _lookup_table_dim = + static_cast(_lookup_table_size) / static_cast(_costmap->getResolution() * _downsampling_factor); // Make sure its a whole number - lookup_table_dim = static_cast(static_cast(lookup_table_dim)); + _lookup_table_dim = static_cast(static_cast(_lookup_table_dim)); // Make sure its an odd number - if (static_cast(lookup_table_dim) % 2 == 0) { + if (static_cast(_lookup_table_dim) % 2 == 0) { RCLCPP_INFO( _logger, "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", - lookup_table_dim); - lookup_table_dim += 1.0; + _lookup_table_dim); + _lookup_table_dim += 1.0; } // Initialize collision checker _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); _collision_checker.setFootprint( - costmap_ros->getRobotFootprint(), - costmap_ros->getUseRadius(), - findCircumscribedCost(costmap_ros)); + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); // Initialize A* template - _a_star = std::make_unique>(motion_model, search_info); + _a_star = std::make_unique>(_motion_model, _search_info); _a_star->initialize( - allow_unknown, - max_iterations, + _allow_unknown, + _max_iterations, std::numeric_limits::max(), - lookup_table_dim, + _lookup_table_dim, _angle_quantizations); // Initialize path smoother @@ -176,20 +175,30 @@ void SmacPlannerHybrid::configure( // Initialize costmap downsampler if (_downsample_costmap && _downsampling_factor > 1) { - std::string topic_name = "downsampled_costmap"; _costmap_downsampler = std::make_unique(); + std::string topic_name = "downsampled_costmap"; _costmap_downsampler->on_configure( node, _global_frame, topic_name, _costmap, _downsampling_factor); } _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + // Setup callback for changes to parameters. + _parameters_client = std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + _parameter_event_sub = _parameters_client->on_parameter_event( + std::bind(&SmacPlannerHybrid::on_parameter_event_callback, this, _1)); + RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerHybrid with " "maximum iterations %i, and %s. Using motion model: %s.", - _name.c_str(), max_iterations, - allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(motion_model).c_str()); + _name.c_str(), _max_iterations, + _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(_motion_model).c_str()); } void SmacPlannerHybrid::activate() @@ -221,8 +230,10 @@ void SmacPlannerHybrid::cleanup() _name.c_str()); _a_star.reset(); _smoother.reset(); - _costmap_downsampler->on_cleanup(); - _costmap_downsampler.reset(); + if (_costmap_downsampler) { + _costmap_downsampler->on_cleanup(); + _costmap_downsampler.reset(); + } _raw_plan_publisher.reset(); } @@ -230,6 +241,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { + std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); std::unique_lock lock(*(_costmap->getMutex())); @@ -251,6 +263,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); } + // This is needed to handle precision issues + if (orientation_bin >= static_cast(_angle_quantizations)) { + orientation_bin -= static_cast(_angle_quantizations); + } unsigned int orientation_bin_id = static_cast(floor(orientation_bin)); _a_star->setStart(mx, my, orientation_bin_id); @@ -260,6 +276,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( while (orientation_bin < 0.0) { orientation_bin += static_cast(_angle_quantizations); } + // This is needed to handle precision issues + if (orientation_bin >= static_cast(_angle_quantizations)) { + orientation_bin -= static_cast(_angle_quantizations); + } orientation_bin_id = static_cast(floor(orientation_bin)); _a_star->setGoal(mx, my, orientation_bin_id); @@ -338,6 +358,158 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( return plan; } +void SmacPlannerHybrid::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + std::lock_guard lock_reinit(_mutex); + + bool reinit_collision_checker = false; + bool reinit_a_star = false; + bool reinit_downsampler = false; + bool reinit_smoother = false; + + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == _name + ".max_planning_time") { + _max_planning_time = value.double_value; + } else if (name == _name + ".lookup_table_size") { + reinit_a_star = true; + _lookup_table_size = value.double_value; + } else if (name == _name + ".minimum_turning_radius") { + reinit_a_star = true; + reinit_smoother = true; + _search_info.minimum_turning_radius = static_cast(value.double_value); + } else if (name == _name + ".reverse_penalty") { + reinit_a_star = true; + _search_info.reverse_penalty = static_cast(value.double_value); + } else if (name == _name + ".change_penalty") { + reinit_a_star = true; + _search_info.change_penalty = static_cast(value.double_value); + } else if (name == _name + ".non_straight_penalty") { + reinit_a_star = true; + _search_info.non_straight_penalty = static_cast(value.double_value); + } else if (name == _name + ".cost_penalty") { + reinit_a_star = true; + _search_info.cost_penalty = static_cast(value.double_value); + } else if (name == _name + ".analytic_expansion_ratio") { + reinit_a_star = true; + _search_info.analytic_expansion_ratio = static_cast(value.double_value); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == _name + ".downsample_costmap") { + reinit_downsampler = true; + _downsample_costmap = value.bool_value; + } else if (name == _name + ".allow_unknown") { + reinit_a_star = true; + _allow_unknown = value.bool_value; + } else if (name == _name + ".cache_obstacle_heuristic") { + reinit_a_star = true; + _search_info.cache_obstacle_heuristic = value.bool_value; + } + } else if (type == ParameterType::PARAMETER_INTEGER) { + if (name == _name + ".downsampling_factor") { + reinit_a_star = true; + reinit_downsampler = true; + _downsampling_factor = value.integer_value; + } else if (name == _name + ".max_iterations") { + reinit_a_star = true; + _max_iterations = value.integer_value; + if (_max_iterations <= 0) { + RCLCPP_INFO( + _logger, "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + } else if (name == _name + ".angle_quantization_bins") { + reinit_collision_checker = true; + reinit_a_star = true; + int angle_quantizations = value.integer_value; + _angle_bin_size = 2.0 * M_PI / angle_quantizations; + _angle_quantizations = static_cast(angle_quantizations); + } + } else if (type == ParameterType::PARAMETER_STRING) { + if (name == _name + ".motion_model_for_search") { + reinit_a_star = true; + _motion_model = fromString(value.string_value); + if (_motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + _motion_model_for_search.c_str()); + } + } + } + } + + // Re-init if needed with mutex lock (to avoid re-init while creating a plan) + if (reinit_a_star || reinit_downsampler || reinit_collision_checker || reinit_smoother) { + // convert to grid coordinates + const double minimum_turning_radius_global_coords = _search_info.minimum_turning_radius; + _search_info.minimum_turning_radius = + _search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); + _lookup_table_dim = + static_cast(_lookup_table_size) / + static_cast(_costmap->getResolution() * _downsampling_factor); + + // Make sure its a whole number + _lookup_table_dim = static_cast(static_cast(_lookup_table_dim)); + + // Make sure its an odd number + if (static_cast(_lookup_table_dim) % 2 == 0) { + RCLCPP_INFO( + _logger, + "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", + _lookup_table_dim); + _lookup_table_dim += 1.0; + } + + // Re-Initialize A* template + if (reinit_a_star) { + _a_star = std::make_unique>(_motion_model, _search_info); + _a_star->initialize( + _allow_unknown, + _max_iterations, + std::numeric_limits::max(), + _lookup_table_dim, + _angle_quantizations); + } + + // Re-Initialize costmap downsampler + if (reinit_downsampler) { + if (_downsample_costmap && _downsampling_factor > 1) { + auto node = _node.lock(); + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); + } + } + + // Re-Initialize collision checker + if (reinit_collision_checker) { + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); + } + + // Re-Initialize smoother + if (reinit_smoother) { + auto node = _node.lock(); + SmootherParams params; + params.get(node, _name); + _smoother = std::make_unique(params); + _smoother->initialize(minimum_turning_radius_global_coords); + } + } +} + } // namespace nav2_smac_planner #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 17e10708f21..c485df5f2ba 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -164,6 +164,47 @@ TEST(AStarTest, test_a_star_se2) delete costmapA; } +TEST(AStarTest, test_se2_single_pose_path) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // in grid coordinates + unsigned int size_theta = 72; + info.cost_penalty = 1.7; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::DUBIN, info); + int max_iterations = 100; + float tolerance = 10.0; + int it_on_approach = 10; + int num_it = 0; + + a_star.initialize(false, max_iterations, it_on_approach, 401, size_theta); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + + std::unique_ptr checker = + std::make_unique(costmapA, size_theta); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + // functional case testing + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + // Goal is one costmap cell away + a_star.setGoal(11u, 10u, 0u); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + + // Check that the path is length one + // With the current implementation, this produces a longer path + // EXPECT_EQ(path.size(), 1u); + EXPECT_GE(path.size(), 1u); + + delete costmapA; +} + TEST(AStarTest, test_constants) { nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index f3391d42404..0509a7cebd7 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -13,21 +13,22 @@ // limitations under the License. Reserved. #include + #include #include #include +#include "geometry_msgs/msg/pose_stamped.hpp" #include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/collision_checker.hpp" -#include "nav2_smac_planner/smac_planner_hybrid.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" class RclCppFixture { @@ -41,8 +42,7 @@ RclCppFixture g_rclcppfixture; // (covered by more extensively testing in other files) // System tests in nav2_system_tests will actually plan with this work -TEST(SmacTest, test_smac_2d) -{ +TEST(SmacTest, test_smac_2d) { rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = std::make_shared("Smac2DTest"); @@ -61,7 +61,10 @@ TEST(SmacTest, test_smac_2d) start.pose.position.x = 0.0; start.pose.position.y = 0.0; start.pose.orientation.w = 1.0; - goal = start; + // goal = start; + goal.pose.position.x = 7.0; + goal.pose.position.y = 0.0; + goal.pose.orientation.w = 1.0; auto planner_2d = std::make_unique(); planner_2d->configure(node2D, "test", nullptr, costmap_ros); planner_2d->activate(); @@ -78,3 +81,59 @@ TEST(SmacTest, test_smac_2d) node2D.reset(); costmap_ros.reset(); } + +TEST(SmacTest, test_smac_2d_reconfigure) { + rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = + std::make_shared("Smac2DTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + auto planner_2d = std::make_unique(); + planner_2d->configure(node2D, "test", nullptr, costmap_ros); + planner_2d->activate(); + + auto rec_param = std::make_shared( + node2D->get_node_base_interface(), node2D->get_node_topics_interface(), + node2D->get_node_graph_interface(), + node2D->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.tolerance", 1.0), + rclcpp::Parameter("test.cost_travel_multiplier", 1.0), + rclcpp::Parameter("test.max_planning_time", 2.0), + rclcpp::Parameter("test.downsample_costmap", false), + rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.downsampling_factor", 2), + rclcpp::Parameter("test.max_iterations", -1), + rclcpp::Parameter("test.max_on_approach_iterations", -1), + rclcpp::Parameter("test.motion_model_for_search", "UNKNOWN")}); + + rclcpp::spin_until_future_complete( + node2D->get_node_base_interface(), + results); + + EXPECT_EQ(node2D->get_parameter("test.tolerance").as_double(), 1.0); + EXPECT_EQ( + node2D->get_parameter("test.cost_travel_multiplier").as_double(), + 1.0); + EXPECT_EQ(node2D->get_parameter("test.max_planning_time").as_double(), 2.0); + EXPECT_EQ(node2D->get_parameter("test.downsample_costmap").as_bool(), false); + EXPECT_EQ(node2D->get_parameter("test.allow_unknown").as_bool(), false); + EXPECT_EQ(node2D->get_parameter("test.downsampling_factor").as_int(), 2); + EXPECT_EQ(node2D->get_parameter("test.max_iterations").as_int(), -1); + EXPECT_EQ( + node2D->get_parameter("test.max_on_approach_iterations").as_int(), + -1); + EXPECT_EQ( + node2D->get_parameter("test.motion_model_for_search").as_string(), + "UNKNOWN"); + + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.downsample_costmap", true)}); + + rclcpp::spin_until_future_complete( + node2D->get_node_base_interface(), + results); +} diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 9c6d50a0b51..f09139055e3 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -79,3 +79,61 @@ TEST(SmacTest, test_smac_se2) costmap_ros.reset(); nodeSE2.reset(); } + +TEST(SmacTest, test_smac_se2_reconfigure) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 = + std::make_shared("SmacSE2Test"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + auto planner = std::make_unique(); + planner->configure(nodeSE2, "test", nullptr, costmap_ros); + planner->activate(); + + auto rec_param = std::make_shared( + nodeSE2->get_node_base_interface(), nodeSE2->get_node_topics_interface(), + nodeSE2->get_node_graph_interface(), + nodeSE2->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.downsample_costmap", true), + rclcpp::Parameter("test.downsampling_factor", 2), + rclcpp::Parameter("test.angle_quantization_bins", 100), + rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.max_iterations", -1), + rclcpp::Parameter("test.minimum_turning_radius", 1.0), + rclcpp::Parameter("test.cache_obstacle_heuristic", true), + rclcpp::Parameter("test.reverse_penalty", 5.0), + rclcpp::Parameter("test.change_penalty", 1.0), + rclcpp::Parameter("test.non_straight_penalty", 2.0), + rclcpp::Parameter("test.cost_penalty", 2.0), + rclcpp::Parameter("test.analytic_expansion_ratio", 4.0), + rclcpp::Parameter("test.max_planning_time", 10.0), + rclcpp::Parameter("test.lookup_table_size", 30.0), + rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); + + rclcpp::spin_until_future_complete( + nodeSE2->get_node_base_interface(), + results); + + EXPECT_EQ(nodeSE2->get_parameter("test.downsample_costmap").as_bool(), true); + EXPECT_EQ(nodeSE2->get_parameter("test.downsampling_factor").as_int(), 2); + EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 100); + EXPECT_EQ(nodeSE2->get_parameter("test.allow_unknown").as_bool(), false); + EXPECT_EQ(nodeSE2->get_parameter("test.max_iterations").as_int(), -1); + EXPECT_EQ(nodeSE2->get_parameter("test.minimum_turning_radius").as_double(), 1.0); + EXPECT_EQ(nodeSE2->get_parameter("test.cache_obstacle_heuristic").as_bool(), true); + EXPECT_EQ(nodeSE2->get_parameter("test.reverse_penalty").as_double(), 5.0); + EXPECT_EQ(nodeSE2->get_parameter("test.change_penalty").as_double(), 1.0); + EXPECT_EQ(nodeSE2->get_parameter("test.non_straight_penalty").as_double(), 2.0); + EXPECT_EQ(nodeSE2->get_parameter("test.cost_penalty").as_double(), 2.0); + EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_ratio").as_double(), 4.0); + EXPECT_EQ(nodeSE2->get_parameter("test.max_planning_time").as_double(), 10.0); + EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0); + EXPECT_EQ( + nodeSE2->get_parameter("test.motion_model_for_search").as_string(), + std::string("REEDS_SHEPP")); +} diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 99eeac63185..1ad9ff36fba 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.0.6 + 1.0.7 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index ca0b585b665..0a611162545 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -210,7 +210,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg('Goal failed with status code: {0}'.format(status)) + self.info_msg(f'Goal failed with status code: {status}') return False self.info_msg('Goal succeeded!') @@ -230,8 +230,7 @@ def checkKeepout(self, x, y): self.warn_msg('Filter mask was not received') elif self.isInKeepout(x, y): self.filter_test_result = False - self.error_msg('Pose (' + str(x) + ', ' + - str(y) + ') belongs to keepout zone') + self.error_msg(f'Pose ({x}, {y}) belongs to keepout zone') return False return True @@ -286,7 +285,7 @@ def dwbCostCloudCallback(self, msg): self.cost_cloud_received = True def speedLimitCallback(self, msg): - self.info_msg('Received speed limit: ' + str(msg.speed_limit)) + self.info_msg(f'Received speed limit: {msg.speed_limit}') self.checkSpeed(self.speed_it, msg.speed_limit) self.speed_it += 1 @@ -340,25 +339,25 @@ def distanceFromGoal(self): d_x = self.current_pose.position.x - self.goal_pose.position.x d_y = self.current_pose.position.y - self.goal_pose.position.y distance = math.sqrt(d_x * d_x + d_y * d_y) - self.info_msg('Distance from goal is: ' + str(distance)) + self.info_msg(f'Distance from goal is: {distance}') return distance def wait_for_node_active(self, node_name: str): # Waits for the node within the tester namespace to become active - self.info_msg('Waiting for ' + node_name + ' to become active') - node_service = node_name + '/get_state' + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' state_client = self.create_client(GetState, node_service) while not state_client.wait_for_service(timeout_sec=1.0): - self.info_msg(node_service + ' service not available, waiting...') + self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' while (state != 'active'): - self.info_msg('Getting ' + node_name + ' state...') + self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: state = future.result().current_state.label - self.info_msg('Result of get_state: %s' % state) + self.info_msg(f'Result of get_state: {state}') else: self.error_msg('Exception while calling service: %r' % future.exception()) @@ -372,8 +371,7 @@ def shutdown(self): mgr_client = self.create_client( ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + - ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -385,13 +383,12 @@ def shutdown(self): self.info_msg( 'Shutting down navigation lifecycle manager complete.') except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') transition_service = 'lifecycle_manager_localization/manage_nodes' mgr_client = self.create_client( ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + - ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -403,7 +400,7 @@ def shutdown(self): self.info_msg( 'Shutting down localization lifecycle manager complete') except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') def wait_for_initial_pose(self): self.initial_pose_received = False diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt index 9c67eadddf7..8abf2611928 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -46,11 +46,11 @@ ament_add_test(test_planner_random TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm ) -ament_add_gtest(test_planner_plugin_failures +ament_add_gtest(test_planner_plugins test_planner_plugins.cpp ) -ament_target_dependencies(test_planner_plugin_failures rclcpp geometry_msgs nav2_msgs ${dependencies}) -target_link_libraries(test_planner_plugin_failures +ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${dependencies}) +target_link_libraries(test_planner_plugins stdc++fs ) diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index c99c1c36bbd..5d4b52eb870 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -20,6 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "planner_tester.hpp" #include "nav2_util/lifecycle_utils.hpp" +#include "nav2_util/geometry_utils.hpp" using namespace std::chrono_literals; @@ -33,6 +34,60 @@ void callback(const nav_msgs::msg::Path::ConstSharedPtr /*grid*/) { } +void testSmallPathValidityAndOrientation(std::string plugin, double length) +{ + auto obj = std::make_shared(); + rclcpp_lifecycle::State state; + obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); + obj->declare_parameter( + "GridBased.use_final_approach_orientation", rclcpp::ParameterValue(false)); + obj->onConfigure(state); + + geometry_msgs::msg::PoseStamped start; + geometry_msgs::msg::PoseStamped goal; + + start.pose.position.x = 0.5; + start.pose.position.y = 0.5; + start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2); + start.header.frame_id = "map"; + + goal.pose.position.x = 0.5; + goal.pose.position.y = start.pose.position.y + length; + goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); + goal.header.frame_id = "map"; + + // Test without use_final_approach_orientation + // expecting end path pose orientation to be equal to goal orientation + auto path = obj->getPlan(start, goal, "GridBased"); + EXPECT_GT((int)path.poses.size(), 0); + EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01); + obj->onCleanup(state); + + // Test WITH use_final_approach_orientation + // expecting end path pose orientation to be equal to approach orientation + // which in the one pose corner case should be the start pose orientation + obj->set_parameter(rclcpp::Parameter("GridBased.use_final_approach_orientation", true)); + obj->onConfigure(state); + path = obj->getPlan(start, goal, "GridBased"); + EXPECT_GT((int)path.poses.size(), 0); + + int path_size = path.poses.size(); + if (path_size == 1) { + EXPECT_NEAR( + tf2::getYaw(path.poses.back().pose.orientation), + tf2::getYaw(start.pose.orientation), + 0.01); + } else { + double dx = path.poses.back().pose.position.x - path.poses.front().pose.position.x; + double dy = path.poses.back().pose.position.y - path.poses.front().pose.position.y; + EXPECT_NEAR( + tf2::getYaw(path.poses.back().pose.orientation), + atan2(dy, dx), + 0.01); + } + obj->onCleanup(state); +} + TEST(testPluginMap, Failures) { auto obj = std::make_shared(); @@ -55,6 +110,81 @@ TEST(testPluginMap, Failures) obj->onCleanup(state); } +TEST(testPluginMap, Smac2dEqualStartGoal) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.0); +} + +TEST(testPluginMap, Smac2dVerySmallPath) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.00001); +} + +TEST(testPluginMap, Smac2dBelowCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.09); +} + +TEST(testPluginMap, Smac2dJustAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.102); +} + +TEST(testPluginMap, Smac2dAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 1.5); +} + +TEST(testPluginMap, NavFnEqualStartGoal) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.0); +} + +TEST(testPluginMap, NavFnVerySmallPath) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.00001); +} + +TEST(testPluginMap, NavFnBelowCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.09); +} + +TEST(testPluginMap, NavFnJustAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.102); +} + +TEST(testPluginMap, NavFnAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 1.5); +} + +TEST(testPluginMap, ThetaStarEqualStartGoal) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.0); +} + +TEST(testPluginMap, ThetaStarVerySmallPath) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.00001); +} + +TEST(testPluginMap, ThetaStarBelowCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.09); +} + +TEST(testPluginMap, ThetaStarJustAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.102); +} + +TEST(testPluginMap, ThetaStarAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py index c25e26d49eb..ac7356cfa4e 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -111,7 +111,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg('Goal failed with status code: {0}'.format(status)) + self.info_msg(f'Goal failed with status code: {status}') return False self.info_msg('Goal succeeded!') @@ -124,22 +124,22 @@ def poseCallback(self, msg): def wait_for_node_active(self, node_name: str): # Waits for the node within the tester namespace to become active - self.info_msg('Waiting for ' + node_name + ' to become active') - node_service = node_name + '/get_state' + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' state_client = self.create_client(GetState, node_service) while not state_client.wait_for_service(timeout_sec=1.0): - self.info_msg(node_service + ' service not available, waiting...') + self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' while (state != 'active'): - self.info_msg('Getting ' + node_name + ' state...') + self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: state = future.result().current_state.label - self.info_msg('Result of get_state: %s' % state) + self.info_msg(f'Result of get_state: {state}') else: - self.error_msg('Exception while calling service: %r' % future.exception()) + self.error_msg(f'Exception while calling service: {future.exception()!r}') time.sleep(5) def shutdown(self): @@ -149,7 +149,7 @@ def shutdown(self): transition_service = 'lifecycle_manager_navigation/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -160,11 +160,11 @@ def shutdown(self): future.result() self.info_msg('Shutting down navigation lifecycle manager complete.') except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') transition_service = 'lifecycle_manager_localization/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -175,7 +175,7 @@ def shutdown(self): future.result() self.info_msg('Shutting down localization lifecycle manager complete') except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') def wait_for_initial_pose(self): self.initial_pose_received = False diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 176f2e44908..2f7aca1b5c0 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -133,14 +133,14 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.error_msg('Failed GROOT_BT - Get Status from ZMQ Publisher') future_return = False except Exception as e: # noqa: B902 - self.error_msg('Failed GROOT_BT - ZMQ Tests: ' + e.__doc__ + e.message) + self.error_msg(f'Failed GROOT_BT - ZMQ Tests: {e}') future_return = False self.info_msg("Waiting for 'NavigateToPose' action to complete") rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg('Goal failed with status code: {0}'.format(status)) + self.info_msg(f'Goal failed with status code: {status}') return False if not future_return: @@ -159,8 +159,8 @@ def grootMonitoringReloadTree(self): sock.setsockopt(zmq.RCVTIMEO, 1000) # sock.setsockopt(zmq.LINGER, 0) - sock.connect('tcp://localhost:' + str(port)) - self.info_msg('ZMQ Server Port: ' + str(port)) + sock.connect(f'tcp://localhost: {port}') + self.info_msg(f'ZMQ Server Port: {port}') # this should fail try: @@ -206,7 +206,7 @@ def grootMonitoringGetStatus(self): # Define subscription and messages with prefix to accept. sock.setsockopt_string(zmq.SUBSCRIBE, '') port = 1666 # default publishing port for groot monitoring - sock.connect('tcp://127.0.0.1:' + str(port)) + sock.connect(f'tcp://127.0.0.1:{port}') for request in range(3): try: @@ -242,27 +242,27 @@ def distanceFromGoal(self): d_x = self.current_pose.position.x - self.goal_pose.position.x d_y = self.current_pose.position.y - self.goal_pose.position.y distance = math.sqrt(d_x * d_x + d_y * d_y) - self.info_msg('Distance from goal is: ' + str(distance)) + self.info_msg(f'Distance from goal is: {distance}') return distance def wait_for_node_active(self, node_name: str): # Waits for the node within the tester namespace to become active - self.info_msg('Waiting for ' + node_name + ' to become active') - node_service = node_name + '/get_state' + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' state_client = self.create_client(GetState, node_service) while not state_client.wait_for_service(timeout_sec=1.0): - self.info_msg(node_service + ' service not available, waiting...') + self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' while (state != 'active'): - self.info_msg('Getting ' + node_name + ' state...') + self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: state = future.result().current_state.label - self.info_msg('Result of get_state: %s' % state) + self.info_msg(f'Result of get_state: {state}') else: - self.error_msg('Exception while calling service: %r' % future.exception()) + self.error_msg(f'Exception while calling service: {future.exception()!r}') time.sleep(5) def shutdown(self): @@ -272,7 +272,7 @@ def shutdown(self): transition_service = 'lifecycle_manager_navigation/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -283,11 +283,11 @@ def shutdown(self): future.result() self.info_msg('Shutting down navigation lifecycle manager complete.') except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') transition_service = 'lifecycle_manager_localization/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -298,7 +298,7 @@ def shutdown(self): future.result() self.info_msg('Shutting down localization lifecycle manager complete') except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') def wait_for_initial_pose(self): self.initial_pose_received = False diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index b0104804ef9..ff37ab2af22 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -89,7 +89,7 @@ def generate_launch_description(): # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: - params_file = eval(robot['name'] + '_params_file') + params_file = eval(f"{robot['name']}_params_file") group = GroupAction([ # Instances use the robot's name for namespace diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index 3635f70a1f8..e4b3a9baeb6 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -116,7 +116,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status if status != GoalStatus.STATUS_ABORTED: - self.info_msg('Goal failed with status code: {0}'.format(status)) + self.info_msg(f'Goal failed with status code: {status}') return False self.info_msg('Goal failed, as expected!') @@ -146,27 +146,27 @@ def distanceFromGoal(self): d_x = self.current_pose.position.x - self.goal_pose.position.x d_y = self.current_pose.position.y - self.goal_pose.position.y distance = math.sqrt(d_x * d_x + d_y * d_y) - self.info_msg('Distance from goal is: ' + str(distance)) + self.info_msg(f'Distance from goal is: {distance}') return distance def wait_for_node_active(self, node_name: str): # Waits for the node within the tester namespace to become active - self.info_msg('Waiting for ' + node_name + ' to become active') - node_service = node_name + '/get_state' + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' state_client = self.create_client(GetState, node_service) while not state_client.wait_for_service(timeout_sec=1.0): - self.info_msg(node_service + ' service not available, waiting...') + self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' while (state != 'active'): - self.info_msg('Getting ' + node_name + ' state...') + self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: state = future.result().current_state.label - self.info_msg('Result of get_state: %s' % state) + self.info_msg(f'Result of get_state: {state}') else: - self.error_msg('Exception while calling service: %r' % future.exception()) + self.error_msg(f'Exception while calling service: {future.exception()!r}') time.sleep(5) def shutdown(self): @@ -176,7 +176,7 @@ def shutdown(self): transition_service = 'lifecycle_manager_navigation/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -187,11 +187,11 @@ def shutdown(self): future.result() self.info_msg('Shutting down navigation lifecycle manager complete.') except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') transition_service = 'lifecycle_manager_localization/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -202,7 +202,7 @@ def shutdown(self): future.result() self.info_msg('Shutting down localization lifecycle manager complete') except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') def wait_for_initial_pose(self): self.initial_pose_received = False diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index beb27a0b753..e6d299caa55 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -87,7 +87,7 @@ def run(self, block): rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') if not self.goal_handle.accepted: self.error_msg('Goal rejected') @@ -105,10 +105,10 @@ def run(self, block): status = get_result_future.result().status result = get_result_future.result().result except Exception as e: # noqa: B902 - self.error_msg('Service call failed %r' % (e,)) + self.error_msg(f'Service call failed {e!r}') if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg('Goal failed with status code: {0}'.format(status)) + self.info_msg(f'Goal failed with status code: {status}') return False if len(result.missed_waypoints) > 0: self.info_msg('Goal failed to process all waypoints,' @@ -130,7 +130,7 @@ def shutdown(self): transition_service = 'lifecycle_manager_navigation/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -139,14 +139,14 @@ def shutdown(self): rclpy.spin_until_future_complete(self, future) future.result() except Exception as e: # noqa: B902 - self.error_msg('%s service call failed %r' % (transition_service, e,)) + self.error_msg(f'{transition_service} service call failed {e!r}') - self.info_msg('{} finished'.format(transition_service)) + self.info_msg(f'{transition_service} finished') transition_service = 'lifecycle_manager_localization/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -155,9 +155,9 @@ def shutdown(self): rclpy.spin_until_future_complete(self, future) future.result() except Exception as e: # noqa: B902 - self.error_msg('%s service call failed %r' % (transition_service, e,)) + self.error_msg(f'{transition_service} service call failed {e!r}') - self.info_msg('{} finished'.format(transition_service)) + self.info_msg(f'{transition_service} finished') def cancel_goal(self): cancel_future = self.goal_handle.cancel_goal_async() diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index ed3f810f095..c888d0a6a45 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -13,7 +13,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - ![example.png](img/00-37.png) -The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` +The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` ## Cost Function Details ### Symbols and their meanings @@ -51,8 +51,6 @@ The parameters of the planner are : - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 - ` .w_euc_cost ` : weight applied on the length of the path. - ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs. -- ` .w_heuristic_cost ` : it has been provided to have an admissible heuristic - Below are the default values of the parameters : ``` planner_server: @@ -64,7 +62,6 @@ planner_server: how_many_corners: 8 w_euc_cost: 1.0 w_traversal_cost: 2.0 - w_heuristic_cost: 1.0 ``` ## Usage Notes @@ -76,9 +73,9 @@ This planner uses the costs associated with each cell from the `global_costmap` Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother. -To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. +`w_heuristic_cost` is an internal setting that is not user changable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent. -Usually set `w_heuristic_cost` at the same value as `w_euc_cost` or 1.0 (whichever is lower). Though as a last resort you may increase the value of `w_heuristic_cost` to speed up the planning process, this is not recommended as it might not always lead to shorter query times, but would definitely give you sub optimal paths +To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to. @@ -91,5 +88,5 @@ This planner is recommended to be used with local planners like DWB or TEB (or o While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so because it might increase the query times from the planner. Test the planners performance on the higher cm/px costmaps before making a switch to finer costmaps. -### Possible Applications -This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). +### When to use this planner? +This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). Another use case would be when you have corridors that are misaligned in the map image, in such a case this planner would be able to give straight-line like paths as it considers line of sight and thus avoid giving jagged paths which arises with other planners because of the finite directions of motion they consider. diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index c90b809291c..a3c8a80b100 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -32,6 +32,7 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_theta_star_planner/theta_star.hpp" +#include "nav2_util/geometry_utils.hpp" namespace nav2_theta_star_planner { @@ -59,6 +60,7 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_{rclcpp::get_logger("ThetaStarPlanner")}; std::string global_frame_, name_; + bool use_final_approach_orientation_; std::unique_ptr planner_; diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 67a4839abf7..9e8a63aa90c 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 0.4.0 + 1.0.7 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 437cbd44714..3134315fc83 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -52,9 +52,14 @@ void ThetaStarPlanner::configure( node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(2.0)); node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); + planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0; nav2_util::declare_parameter_if_not_declared( node, name_ + ".w_heuristic_cost", rclcpp::ParameterValue(1.0)); node->get_parameter(name_ + ".w_heuristic_cost", planner_->w_heuristic_cost_); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_); } void ThetaStarPlanner::cleanup() @@ -79,14 +84,63 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( { nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); + + // Corner case of start and goal beeing on the same cell + unsigned int mx_start, my_start, mx_goal, my_goal; + planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); + planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + if (mx_start == mx_goal && my_start == my_goal) { + if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); + return global_path; + } + global_path.header.stamp = clock_->now(); + global_path.header.frame_id = global_frame_; + geometry_msgs::msg::PoseStamped pose; + pose.header = global_path.header; + pose.pose.position.z = 0.0; + + pose.pose = start.pose; + // if we have a different start and goal orientation, set the unique path pose to the goal + // orientation, unless use_final_approach_orientation=true where we need it to be the start + // orientation to avoid movement from the local planner + if (start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_) { + pose.pose.orientation = goal.pose.orientation; + } + global_path.poses.push_back(pose); + return global_path; + } + planner_->setStartAndGoal(start, goal); RCLCPP_DEBUG( logger_, "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); getPlan(global_path); + global_path.poses.back().pose.orientation = goal.pose.orientation; + + // If use_final_approach_orientation=true, interpolate the last pose orientation from the + // previous pose to set the orientation to the 'final approach' orientation of the robot so + // it does not rotate. + // And deal with corner case of plan of length 1 + if (use_final_approach_orientation_) { + size_t plan_size = global_path.poses.size(); + if (plan_size == 1) { + global_path.poses.back().pose.orientation = start.pose.orientation; + } else if (plan_size > 1) { + double dx, dy, theta; + auto last_pose = global_path.poses.back().pose.position; + auto approach_pose = global_path.poses[plan_size - 2].pose.position; + dx = last_pose.x - approach_pose.x; + dy = last_pose.y - approach_pose.y; + theta = atan2(dy, dx); + global_path.poses.back().pose.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(theta); + } + } + auto stop_time = std::chrono::steady_clock::now(); auto dur = std::chrono::duration_cast(stop_time - start_time); - RCLCPP_DEBUG(logger_, "the time taken for pointy is : %i", static_cast(dur.count())); + RCLCPP_DEBUG(logger_, "the time taken is : %i", static_cast(dur.count())); RCLCPP_DEBUG(logger_, "the nodes_opened are: %i", planner_->nodes_opened); return global_path; } @@ -113,15 +167,14 @@ nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( { nav_msgs::msg::Path pa; + geometry_msgs::msg::PoseStamped p1; for (unsigned int j = 0; j < raw_path.size() - 1; j++) { - geometry_msgs::msg::PoseStamped p; coordsW pt1 = raw_path[j]; - p.pose.position.x = pt1.x; - p.pose.position.y = pt1.y; - pa.poses.push_back(p); + p1.pose.position.x = pt1.x; + p1.pose.position.y = pt1.y; + pa.poses.push_back(p1); coordsW pt2 = raw_path[j + 1]; - geometry_msgs::msg::PoseStamped p1; double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); int loops = static_cast(distance / dist_bw_points); double sin_alpha = (pt2.y - pt1.y) / distance; @@ -132,6 +185,7 @@ nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( pa.poses.push_back(p1); } } + return pa; } diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp index 634b4acd5bc..78d84fb8dbf 100644 --- a/nav2_util/include/nav2_util/node_thread.hpp +++ b/nav2_util/include/nav2_util/node_thread.hpp @@ -23,7 +23,7 @@ namespace nav2_util { /** * @class nav2_util::NodeThread - * @brief A background thread to process node callbacks + * @brief A background thread to process node/executor callbacks */ class NodeThread { @@ -34,6 +34,12 @@ class NodeThread */ explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); + /** + * @brief A background thread to process executor's callbacks constructor + * @param executor Interface to executor to spin in thread + */ + explicit NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); + /** * @brief A background thread to process node callbacks constructor * @param node Node pointer to spin in thread @@ -51,7 +57,7 @@ class NodeThread protected: rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_; std::unique_ptr thread_; - rclcpp::executors::SingleThreadedExecutor executor_; + rclcpp::Executor::SharedPtr executor_; }; } // namespace nav2_util diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 3a468fe7828..2edf343c720 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.0.6 + 1.0.7 TODO Michael Jeronimo Mohammad Haghighipanah @@ -39,6 +39,7 @@ launch_testing_ament_cmake std_srvs action_msgs + launch_testing_ros ament_cmake_pytest diff --git a/nav2_util/src/node_thread.cpp b/nav2_util/src/node_thread.cpp index e498e1371d3..c19fbd98477 100644 --- a/nav2_util/src/node_thread.cpp +++ b/nav2_util/src/node_thread.cpp @@ -22,19 +22,25 @@ namespace nav2_util NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) : node_(node_base) { + executor_ = std::make_shared(); thread_ = std::make_unique( [&]() { - executor_.add_node(node_); - executor_.spin(); - executor_.remove_node(node_); + executor_->add_node(node_); + executor_->spin(); + executor_->remove_node(node_); }); } +NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor) +: executor_(executor) +{ + thread_ = std::make_unique([&]() {executor_->spin();}); +} NodeThread::~NodeThread() { - executor_.cancel(); + executor_->cancel(); thread_->join(); } diff --git a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp index de3e30716ff..9e5ea941013 100644 --- a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp +++ b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp @@ -226,18 +226,6 @@ class VoxelGrid double x1, double y1, double z1, unsigned int max_length = UINT_MAX, unsigned int min_length = 0) { - int dx = int(x1) - int(x0); // NOLINT - int dy = int(y1) - int(y0); // NOLINT - int dz = int(z1) - int(z0); // NOLINT - - unsigned int abs_dx = abs(dx); - unsigned int abs_dy = abs(dy); - unsigned int abs_dz = abs(dz); - - int offset_dx = sign(dx); - int offset_dy = sign(dy) * size_x_; - int offset_dz = sign(dz); - // we need to chose how much to scale our dominant dimension, based on the // maximum length of the line double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1) + (z0 - z1) * (z0 - z1)); @@ -247,10 +235,21 @@ class VoxelGrid double scale = std::min(1.0, max_length / dist); // Updating starting point to the point at distance min_length from the initial point - double min_x0 = x0 + dx / dist * min_length; - double min_y0 = y0 + dy / dist * min_length; - double min_z0 = z0 + dz / dist * min_length; + double min_x0 = x0 + (x1 - x0) / dist * min_length; + double min_y0 = y0 + (y1 - y0) / dist * min_length; + double min_z0 = z0 + (z1 - z0) / dist * min_length; + int dx = int(x1) - int(min_x0); // NOLINT + int dy = int(y1) - int(min_y0); // NOLINT + int dz = int(z1) - int(min_z0); // NOLINT + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + unsigned int abs_dz = abs(dz); + + int offset_dx = sign(dx); + int offset_dy = sign(dy) * size_x_; + int offset_dz = sign(dz); unsigned int z_mask = ((1 << 16) | 1) << (unsigned int)min_z0; unsigned int offset = (unsigned int)min_y0 * size_x_ + (unsigned int)min_x0; @@ -258,16 +257,14 @@ class VoxelGrid GridOffset grid_off(offset); ZOffset z_off(z_mask); - unsigned int length = 0; // is x dominant if (abs_dx >= max(abs_dy, abs_dz)) { int error_y = abs_dx / 2; int error_z = abs_dx / 2; - // Since initial point has been updated above, subtracting min_length from the total length - length = (unsigned int)(scale * abs_dx) - min_length; + bresenham3D( at, grid_off, grid_off, z_off, abs_dx, abs_dy, abs_dz, error_y, error_z, - offset_dx, offset_dy, offset_dz, offset, z_mask, length); + offset_dx, offset_dy, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dx)); return; } @@ -275,23 +272,20 @@ class VoxelGrid if (abs_dy >= abs_dz) { int error_x = abs_dy / 2; int error_z = abs_dy / 2; - // Since initial point has been updated above, subtracting min_length from the total length - length = (unsigned int)(scale * abs_dy) - min_length; + bresenham3D( at, grid_off, grid_off, z_off, abs_dy, abs_dx, abs_dz, error_x, error_z, - offset_dy, offset_dx, offset_dz, offset, z_mask, length); + offset_dy, offset_dx, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dy)); return; } // otherwise, z is dominant int error_x = abs_dz / 2; int error_y = abs_dz / 2; - // Since initial point has been updated above, subtracting min_length from the total length - length = (unsigned int)(scale * abs_dz) - min_length; bresenham3D( at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz, - offset_dx, offset_dy, offset, z_mask, length); + offset_dx, offset_dy, offset, z_mask, (unsigned int)(scale * abs_dz)); } private: diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index c10df473303..3e81ed5b315 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.0.6 + 1.0.7 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_voxel_grid/test/CMakeLists.txt b/nav2_voxel_grid/test/CMakeLists.txt index eb68ad8b3ae..ff01e4bde12 100644 --- a/nav2_voxel_grid/test/CMakeLists.txt +++ b/nav2_voxel_grid/test/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(voxel_grid_tests voxel_grid_tests.cpp) target_link_libraries(voxel_grid_tests voxel_grid) + +ament_add_gtest(voxel_grid_bresenham_3d voxel_grid_bresenham_3d.cpp) +target_link_libraries(voxel_grid_bresenham_3d voxel_grid) diff --git a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp new file mode 100644 index 00000000000..10ff23dd87a --- /dev/null +++ b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp @@ -0,0 +1,129 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2021 Samsung Research Russia +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Alexey Merzlyakov +*********************************************************************/ +#include +#include + +class TestVoxel +{ +public: + explicit TestVoxel(uint32_t * data, int sz_x, int sz_y) + : data_(data) + { + size_ = sz_x * sz_y; + } + inline void operator()(unsigned int off, unsigned int val) + { + ASSERT_TRUE(off < size_); + data_[off] = val; + } + +private: + uint32_t * data_; + unsigned int size_; +}; + +TEST(voxel_grid, bresenham3DBoundariesCheck) +{ + const int sz_x = 60; + const int sz_y = 60; + const int sz_z = 2; + const unsigned int max_length = 60; + const unsigned int min_length = 6; + nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z); + TestVoxel tv(vg.getData(), sz_x, sz_y); + + // Initial point - some assymetrically standing point in order to cover most corner cases + const double x0 = 2.2; + const double y0 = 3.8; + const double z0 = 0.4; + // z-axis won't be domimant + const double z1 = 0.5; + // (x1, y1) point will move + double x1, y1; + + // Epsilon for outer boundaries of voxel grid array + const double epsilon = 0.02; + + // Running on (x, 0) edge + y1 = 0.0; + for (int i = 0; i <= sz_x; i++) { + if (i != sz_x) { + x1 = i; + } else { + x1 = i - epsilon; + } + vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length); + } + + // Running on (x, sz_y) edge + y1 = sz_y - epsilon; + for (int i = 0; i <= sz_x; i++) { + if (i != sz_x) { + x1 = i; + } else { + x1 = i - epsilon; + } + vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length); + } + + // Running on (0, y) edge + x1 = 0.0; + for (int j = 0; j <= sz_y; j++) { + if (j != sz_y) { + y1 = j; + } else { + y1 = j - epsilon; + } + vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length); + } + + // Running on (sz_x, y) edge + x1 = sz_x - epsilon; + for (int j = 0; j <= sz_y; j++) { + if (j != sz_y) { + y1 = j; + } else { + y1 = j - epsilon; + } + vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 9407a6585c5..99cb2754e6b 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -104,8 +104,8 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(wait_at_waypoint photo_at_waypoint input_at_waypoint) - +ament_export_libraries(wait_at_waypoint photo_at_waypoint input_at_waypoint ${library_name}) +ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml) ament_package() diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 7429ba3fe2f..ea2f0eac5c7 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.0.6 + 1.0.7 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 889272cc538..3c0e1750239 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.0.6 + 1.0.7 ROS2 Navigation Stack diff --git a/tools/bt2img.py b/tools/bt2img.py index 31c84eecec3..31480f19be3 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -89,7 +89,7 @@ def main(): legend.render(args.legend) dot.format = 'png' if args.save_dot: - print("Saving dot to " + str(args.save_dot)) + print(f'Saving dot to {args.save_dot}') args.save_dot.write(dot.source) dot.render(args.image_out, view=args.display) @@ -119,7 +119,7 @@ def find_behavior_tree(xml_tree, tree_name): if tree_name == tree.get('ID'): return tree - raise RuntimeError("No behavior tree for name " + tree_name + " found in the XML file") + raise RuntimeError(f'No behavior tree for name {tree_name} found in the XML file') # Generate a dot description of the root of the behavior tree. def convert2dot(behavior_tree):